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ABSTRACT 


The main problem addressed by this research is to find an alternative to the use of large 
and/or expensive equipment required by conventional navigation systems to accurately 
determine the position of an Autonomous Underwater Vehicle (AUV) during all phases of 
an underwater search or mapping mission. 

The approach taken was to advance an existing integrated navigation system prototype 
which combines Global Positioning System (GPS), Inertial Measurement Unit (IMU), 
water speed, and heading information using Kalman filtering techniques. The hardware 
and software architecture of the prototype system were advanced to a level such that it is 
completely self-contained in a relatively small, lightweight package capable of on-board 
processing of sensor data and outputting updated position fixes at a rate of 10 Hz; an 
improvement from the 5 Hz rate delivered by the prototype. The major changes to the 
preceding prototype implemented by this research were to install an on-board processor to 
locally process sensor outputs, and improve upon the analog filter and voltage regulation 
circuitry. 

Preliminary test results indicate the newly designed SANS provides a 100% 
performance improvement over the previous prototype. It now delivers a 10 Hz update rate, 
and increased accuracy due to the improved analog filter and the higher sampling rate 


provided by the processor. 
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I. INTRODUCTION 


A. BACKGROUND 


An Autonomous Underwater Vehicle (AUV) can be capable of numerous missions both overt 
and clandestine. Such vehicles have been used for inspection, mine countermeasures, survey, 
observation, etc. [Yuh 95]. Recent research trends in underwater robotics have emphasized 
minimizing the need for human interaction by increasing the autonomy of such vehicles. 

The NPS ‘Phoenix’ AUV is an experimental vehicle designed primarily for research in support 
of shallow-water mine countermeasures and coastal environmental monitoring [Healey 93, 95, 
Brutzman 96]. In [Kwak 93], an approach is described for determining the position of submerged 
detected objects by executing a “pop-up” maneuver to obtain a GPS fix, and then extrapolating this 
fix backwards to the submerged object location using recorded inertial data. As explained in [Kwak 
93], navigation accuracy during such a surfacing maneuver is strongly enhanced by the use of 
accurate depth information available from low-cost pressure cells. However, this form of “aided” 
inertial navigation [Brown 92], is not applicable to a surfaced AUV. Inertial navigation is not 
needed in circumstances where continuous reliable reception of GPS satellite signals is possible. 
However, this does not apply to AUVs, unless perhaps they are fitted with a mast to extend a GPS 
antenna above the effects of wave action. Such a mast is not an attractive option for military 
operations, and in any event may be mechanically difficult. 

In efforts to overcome the problem of intermittent GPS satellite tracking for surfaced (or 
cruising near the surface) AUV navigation, an experimental system, using a low-cost strapped- 
down inertial measurement unit (IMU), has been designed to enable inertial navigation between 
GPS fixes. This system is well suited for pop-up navigation, so finding a means of navigating near 
the surface provides a complete solution to the overall navigation problem associated with 
transiting an AUV to a shallow water work site, recording the position of detected submerged 
objects, and then returning to a recovery site where stored mission data can be uploaded [McGhee 
95}. 

Many of the missions of the Phoenix class of vehicles can be separated into two distinct 
phases: transit and search. After being launched from an aircraft, submarine, or surface vessel, such 


an AUV would conduct the transit phase of the mission in order to arrive at the search area. After 


the search phase, the AUV would transit back to a recovery position. Neither of these transit phases 
require as high a degree of navigation accuracy as the search phase. Once established in the mission 
area, the Phoenix would enter the search phase which might include missions such as mine- 
hunting, mapping, or environmental data collection. Typically, the search phase would require 
more precise navigation which could be provided by more frequent GPS fixes or by using 
Differential GPS (DGPS) or post-processing, if available. Both mission phases may involve 
waypoint steering and obstacle avoidance. 

One of the most important and difficult aspects of an AUV mission is navigation. It is 
important that the navigation system be robust if the AUV is to be capable of a wide variety of 
missions. In order to achieve such robustness, the AUV should be capable of navigating with the 
Global Positioning System (GPS) and/or an Inertial Navigation System (INS). The GPS is capable 
of highly accurate positioning when the AUV is surfaced, while an INS can be used for submerged 
navigation. In order to ensure accurate navigation for the various missions, the GPS and INS 
components can be combined. A favorable analysis of this type of navigation system was 
conducted in [McKeon 92]. The hardware and software architecture required for a typical mapping 
scenario was evaluated in [Norton 94]. 

[Bachmann 95] made the architecture evaluated in [Norton 94] a reality, and subsequently 
developed the first working prototype of the proposed Shallow-water AUV Navigation System 
(SANS). With the prototype SANS having achieved favorable results in open-water, at-sea test 
trials, the research reported in this thesis advances the SANS to another level of maturity, making 


it now ready for direct application to a real-world AUV. 


B. RESEARCH QUESTIONS 

This thesis will examine the following research topics: 

- Evaluate the hardware and software architecture of the GPS/INS prototype SANS. 

- Evaluate the feasibility of an AUV accurately navigating from point to point using GPS/INS 
while conducting open-ocean transit. 

- Develop a hardware configuration which will enable the GPS/INS SANS to be housed in 


one small, self-contained package. 


C. SCOPE, LIMITATIONS AND ASSUMPTIONS 


This thesis reports the findings of the fifth year of research in an ongoing research project. The 
scope of this thesis is to investigate the feasibility of an AUV navigating from point to point using 
a combination of GPS/INS. The requirements for a SANS described by [Kwak 93] which impact 
this project are: 

- Low power consumption. Operation from an appropriately sized external battery pack for 12 
hours is desirable. 

- Limited exposure time. The amount of time that the GPS antenna is exposed in the search 
phase should be as short as possible. Up to 30 seconds of exposure is allowed, but time between 
exposures should be maximized. 

- Maintain clandestine operation. The GPS antenna should present a very small cross section 
when exposed and should not extend more than a few inches above the surface of the water. 

- Maximize accuracy. During the search phase of the mission, system accuracy of 10 meters 
or better is required with postprocessing, both submerged and surfaced. 

- Total volume not to exceed 120 cubic inches. Elongated, streamlined packaging is preferred. 

For the purposes of this research, DGPS will be used as ground truth data (without 
postprocessing) for determining appropriate Kalman filter gains. However, some real-world 
scenarios will only utilize the noncorrected GPS signal for real-time mission navigation and may 


require further tuning of Kalman filter coefficients. 


D. ORGANIZATION OF THESIS 


The purpose of this thesis is to present the development of a system meeting the mission 
requirements of the SANS. The term AUV is understood to include any small underwater vehicle 
(including human divers) which can easily carry such a compact device. The term “towfish”’ refers 
to the test vehicle (depicted in Figure 2.2) used to evaluate the SANS during at-sea testing. 

Chapter II reviews previous work on this project as well as previous work on GPS navigation 
and AUV submerged navigation. 

Chapter III provides an evaluation of the prototype SANS in the form of both a time domain 


and frequency domain analysis of the IMU sensor output signals. 


Chapter IV is a detailed description of the hardware currently in use for this portion of the 
project. 

Chapter V is a detailed description of the software changes, additions, and updates made to 
support this portion of the project. The chapter includes a description of the C++ code required for 
future towfish experiments. This description provides an explanation of the class and object 
hierarchy used, as well as an outline of the major functions that were added as a result of this 
research. 

Chapter VI is a description of the experiment design and analysis of the experimental results. 


Chapter VII presents the thesis conclusions and provides recommendations for future 


research. 


Il. SURVEY OF RELATED WORK 


A. INTRODUCTION 

Autonomous Underwater Vehicles (AUVs) have the potential to be used in an efficient and 
cost effective manner in a variety of missions involving military and non-military applications. 
One of the most important aspects of an AUV mission is the ability to navigate accurately. Many 
possible missions, such as mine-hunting, require a high degree of navigation accuracy. This chap- 
ter will discuss some of the solutions for navigating an AUV. 

In general there are two categories of navigation systems: those that are based on external 
signals and those that are based on sensors. External-signal-based navigation systems such as, 
Loran, Omega, and GPS are only able to determine position while the signal receiver is exposed 
to the signal. Loran and Omega are relatively inaccurate compared to GPS. While Loran covers 
almost the entire northern hemisphere, it has almost no coverage in the southern hemisphere 
[Bowditch 84]. GPS on the other hand is capable of world-wide coverage with a high degree of 
navigational accuracy.[Logsdon 92] 

Sensor-based navigation can be implemented as a self-contained unit which can be composed 
of various types of equipment such as IMUs, acoustic transponders, or geophysical map 
comparison. Each of these components has its disadvantages. Acoustic transponders must be pre- 
deployed at precisely known locations and may require costly maintenance. Geophysical map 
interrogation requires a precise bottom contour map previously stored in the AUV’s computer. 


IMU-based navigation is prone to sensor drift, which if left uncorrected, can become very large. 


B. GPS NAVIGATION 

The Navigation Satellite Timing and Ranging (NAVSTAR) Global Positioning System (GPS) 
is a space-based radio positioning, navigation and time-transfer system sponsored by the U.S. 
Department of Defense (DoD). It was originally intended to provide the military with precise nav- 
igation and timing capabilities [Parkinson 80]. The system is designed to provide 24-hour, all- 


weather navigation by providing total earth coverage using 24 satellites in 22,200 km orbits that 


are inclined 55°, with 12 hour periods. The satellites broadcast two L-band frequencies: L1 
(1575.4 MHz) and L2 (1227.6 MHz). Navigation and system data, predicted satellite position 


(ephemeris) information, atmospheric propagation correction data, satellite clock error informa- 


tion, and satellite health data are all superimposed on these two carrier frequencies [Logsdon 92, 
Wooden 85]. 

There are two different navigation services available from the GPS satellites depending on the 
type of receiver being used: the Standard Positioning Service (SPS) and the Precise Positioning 
Service (PPS). The SPS is achieved by receiving the L1 carrier signal which is broadcast with an 
intentional inaccuracy called Selective Availability (SA). SA limits world-wide navigation to 100 
m horizontal accuracy with a 95% confidence level [Logsdon 92]. The PPS is limited to U.S. and 
allied military, and specific non-military uses that are in the national interest. Access to PPS is 
restricted by use of special cryptographic equipment. PPS provides the highest stand alone 
accuracy: 15 m Spherical Error Probable (SEP), a velocity accuracy of 0.1 m/sec, and a timing 
accuracy of better than 100 nanoseconds [Logsdon 92, Wooden 85]. 

In order to take full advantage of GPS precision without having access to cryptographic 
equipment, civilian customers have determined a way to improve the accuracy of the SPS. The 
most common way to work around the inaccuracies of the SPS is Differential GPS (DGPS), which 
may be used in real-time or during post-processing. DGPS is a method which allows highly 
accurate information to be obtained from GPS without the cryptographic equipment required for 
access to the P-code of PPS. The idea behind DGPS 1s to survey a receiver at a stationary site, allow 
the stationary site to determine the difference between its actual position and its GPS position, and 
broadcast the pseudorange corrections to any DGPS capable receivers. Real-time differential 
processing can reduce the typical 100 m accuracy of the SPS to 2-4 m regardless of the status of 
SA [Logsdon 92]. In the case of post-processing, itis possible to have the AUV record the raw PPS 
or SPS GPS information for later comparison to a known geographical site. Precise post- 
processing procedures can be used to reconstruct extremely accurate positioning information, 


typically in the submeter range. Table 1 shows a comparison of expected GPS accuracies. 


POSITIONING SERVICE PPS (m) SPS (m) 
Non-Differential 100 


Table 1: Expected RMS GPS accuracy levels [Logsdon 92] 
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As GPS technology has matured, the size and cost of GPS receivers has decreased drastically. 
Not only is miniaturization improving, but GPS receivers have maintained or increased in 
performance capability. Since as early as 1992, the GPS industry has been able to produce 
receivers that are essentially a single printed circuit board. [Souen 92] reports that the Furuno GPS 
receiver module LGN-72 is an eight-channel receiver implemented on a single printed circuit 
board measuring 100 mm x 70 mm x 20 mm and requiring only 2 W of power. 

There 1s currently however, a performance trade-off associated with the miniaturization of 
GPS receivers. Trimble currently offers the PC Card 110 GPS receiver in the form of a Personal 
Memory Card International (PCMCIA) interface. This credit card sized device simply slides into 
any laptop, most palmtops, or pen-based computer compliant with PCMCIA (release 2.0). This 
miniature GPS receiver is capable of tracking eight satellites using three channels. However, 
because it does not have an allocated channel for each of the eight satellites it’s capable of tracking, 
it does not use a continuous tracking scheme, which degrades its acquisition time performance. In 
order to reduce the size of the receiver, manufacturers are reducing the number of channels on the 
receiver. In this configuration, GPS receivers are called “sequencing” receivers [Logsdon 92]. 
Sequencing receivers utilize a time-sharing technique to “dwell” on each satellite for a brief 
interval before switching to the next satellite in the sequence. Sequencing receivers have a typical 
acquisition time of about 2 minutes. Continuous tracking GPS receivers have a typical acquisition 
time of about 30 seconds, however, they are less compact in size as they have more receiver 
channels. Given this trade-off relationship between size and performance, the choice of GPS 
receiver must be made with the particular application in mind. If the application is not so dynamic 
(1.e., mobile navigation), a sequencing receiver would offer an adequate compromise. However, 
if the application requires a short time to initial acquisition, the most viable option is the continuous 
tracking receiver. 

Given the level of miniaturization and performance along with its excellent accuracy, GPS is 
an obvious choice for AUV navigation. One manner of using GPS to locate an AUV is to place 
buoys with GPS receivers at appropriate locations. These buoys would translate the GPS signal and 
retransmit an underwater acoustic signal. The AUV would determine its position via ranging and 
position fixes to the buoys. [Youngberg 91] suggests that the GPS antenna, receiver, processing 
and control subsystem, acoustic transmitter, battery power, and homing beacon could all be 


contained in a buoy measuring 123 mm diameter x 910 mm long and weighing 5-15 kg. A 


simulation which showed the feasibility of this approach is presented in [Leu 93]. The simulation 
consisted of several sonobuoys spaced one kilometer apart. Due to uncertainties in buoy position 
caused by wave action and variations in altitude, the study proposed the use of Kalman filtering 
techniques to combine the outputs of an accelerometer and DGPS to enhance accuracy. Each GPS 
buoy would essentially act as a GPS satellite and broadcast its position via spread spectrum signals 
used by the AUV for ranging. This technique would eliminate the requirement to predeploy a 
surveyed transponder field. 

Another possible method for using GPS to determine the AUV’s position is to physically 
mount the GPS antenna and receiver onboard the AUV. One major concern would be that the GPS 
receiver would be unable to acquire satellites in a timely manner suitable to the mission due to 
splash effects on the antenna. [Norton 94] describes both static and dynamic test results which 
show that a submersible system is able to meet the accuracy and time requirements of the mission 


while being splashed by wave wash. 


C. INERTIAL NAVIGATION 

Inertial navigation is basically a complex method of dead reckoning. In its purest form it 
involves no outside references to fix position. All position data is calculated relative to a known 
Starting point. An inertial navigation system (INS) continuously measures three mutually orthog- 
onal acceleration components using accelerometers. These measurements are taken in short time 
increments and multiplied by elapsed time in order to determine an estimate of instantaneous 
velocity. The three-dimensional change in position can then be determined by integrating respec- 
tive velocities over time. [Bachmann 95] 

The primary drawback of any INS is the tendency for small sensor drift rates to accumulate 
errors over time. Without outside references for correction, these errors grow relentlessly and 
eventually lead to large errors in the estimated position. Highly accurate inertial navigation 
systems can be constructed, but they are large, costly, and complex [Tuohy 93]. Size alone makes 
them unacceptable candidates for the SANS. In order to meet the SANS requirements, a low-cost 
INS can be integrated with GPS. GPS will provide the INS with the periodic position fixes 
necessary to correct slowly building INS errors. 

The acceleration measurements required by an INS can be made by several types of IMUs. 


These can be divided into two fundamental categories: gimbaled and strapdown. Due to their large 


size and power requirements, gimbaled systems are not suitable for the SANS. In a strapdown unit, 
three mutually orthogonal accelerometers and three angular rate sensors are mounted parallel to 
the three body axes of the vehicle. Changes in linear and rotational velocities are continuously 
measured. Strapdown systems are smaller and simpler than gimbaled systems, but necessitate 


much larger computational capabilities [Logsdon 92]. 


D. INTEGRATED GPS/INS NAVIGATION 

SPS could be used to adequately perform both the transit and search phases of an AUV mis- 
sion. During the transit phases, non-differential SPS and a magnetic compass would provide the 
primary source of navigation data. In order to utilize GPS as a meaningful correction to a low-cost 
INS system, periods between fixes during the transit phase must not exceed the time in which an 
AUV could travel a distance greater than the horizontal accuracy of SPS (100m). The mapping 
phases of an AUV mission would require the vehicle to maintain more accurate navigational pic- 
ture both submerged and on the surface. This would necessitate the use of periodic differentially 
corrected GPS information in order to keep the INS system accurate while submerged. This dif- 
ferential correction could be provided in real-time during overt missions along friendly shores, 
provided a DGPS reference signal is available, or during mission post-processing following a 
clandestine mission. 

Integration of GPS and INS into a single system can produce continuously accurate 
navigational information even when using relatively low-cost components. This integration not 
only allows periodic reinstallation of the INS to correct accumulated errors but can also (with the 
aid of Kalman filtering techniques) improve the performance of the INS between fixes. Filtering 
the acceleration data with additional sensor information such as water speed and heading will 
further improve the quality of the integrated system. Overall, an integrated system will provide 
improved reliability, smaller navigation errors and superior survivability [Logsdon 92]. 

Kalman filtering is a method of combining all available sensor data regardless of their 
precision to estimate the current posture of a vehicle [Cox 90]. The filter is actually a data- 
processing algorithm which minimizes the error of this estimate statistically using currently 
available sensor data and prior knowledge of system characteristics. Each piece of datais weighted 
based upon the expected accuracy of the measurement it represents. In a complementary filter, 


low-frequency data, which is trusted over the long term, and high-frequency data, which is trusted 


only in the short term, are used to “complement” each other providing a much better estimate than 
either can alone [Brown 92]. 

[Bachmann 95] demonstrated the use of this complementary filter technique by combining the 
low-frequency data of the accelerometers and compass with high-frequency angular rate and 
heading information. Intermediate position results were obtained by integrating high-frequency 
water-speed data. GPS data was used to reinitialize the system each time a fix was obtained and 
develop an error bias, expressed as an apparent current, to correct the system between fixes. The 
concept of using the relatively inexpensive IMU with limited accuracy coupled with differentially- 
corrected GPS has proven to be a viable solution to the challenge of shallow-water AUV 


navigation [Bachmann 95]. 


E. AUV SUBMERGED NAVIGATION 

There are many techniques available for submerged navigation, including dead reckoning, 
inertial, electromagnetic and acoustic navigation. With acoustic navigation, time of arrival and 
direction of propagation of acoustic waves are the two principal measurements made. A wide 
variety of acoustic navigation systems have been developed for underwater vehicle use. They are 
typically divided into long, short, and ultrashort baseline systems (LBL, SBL, and USBL). All 
involve the use of an array of acoustic beacons or receivers whose positions must be known to an 
accuracy better than the desired vehicle localization accuracy [Tuohy 93]. Unfortunately, most 
acoustic navigation systems require major expeditions for their accurate set-up and periodic main- 
tenance. This makes them expensive and in many ways reduces the level of autonomy achievable 
by an AUV. Also, acoustic navigation methods are affected by changes in the speed of sound in 
the ocean and suffer from refraction and multipath propagation problems in restricted shallow 
water coastal and ice-covered areas [Tuohy 93]. 

There are various other ways of determining a vehicle’s velocity and position while 
submerged without the aid of external signals. An AUV could use Doppler sonar to determine 
velocity. Charge Coupled Device (CCD) cameras, laser scanning, or variations in the earth’s 
magnetic field can also aid in determining position [Bergem 93]. Position could also be estimated 
by the double integration of acceleration as sensed by an Inertial Measurement Unit (IMU). 

Unless an AUV has access to outside references, it will not be able to refer to external signals 


while submerged. If this is the case, then the system must rely on some sort of dead reckoning. 
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Modern dead reckoning systems typically use magnetic or gyroscopic heading sensors and a 
bottom or water-locked velocity sensor [Grose 92]. The main problem is that the presence of an 
ocean current will add a velocity component to the vehicle which is not detected by the speed log. 
In the vicinity of the shore, ocean currents can exceed two knots [Tuohy 93]. Using dead reckoning 
with currents which are relatively large in relation to the typical 4-6 knot speed of an AUV can 
produce extremely inaccurate results [Tuohy 93]. 

There are many techniques for measuring accelerations and angular rates. These include using 
ring laser and fiber optic gyros, rotating mass gyros, vibratory rate sensors, and high performance 
IMUs. Inertial grade IMUs typically contain three angular rate sensors, three precision linear 
accelerometers and a three-axis magnetometer. The acceleration measurements required by an 
Inertial Navigation System (INS) can be made by several types of [MUs. Again, these can be 
divided into two fundamental categories: gimbaled and strapdown. All of these sensors are subject 
to drift errors which relentlessly increase with time. High quality sensors are subject to less drift 
but can cost up to $100,000 [Tuohy 93], making them unattractive for small AUVs. 

[McKeon 92] proposes a combination of GPS and INS to allow an AUV to determine position 
information. While submerged, the AUV uses a low-cost inertial navigation system. However, 
when on the surface the vehicle has access to GPS information. GPS/INS information could be 
combined with a Kalman filter techniques to reduce the errors during the next dive sequence as 


simulated in [Nagengast 92] and demonstrated in [McGhee 95]. 


F, THE PREVIOUS PROTOTYPE 

[Bachmann 95] describes in detail the previous hardware and software configurations of the 
SANS. For the benefit of the reader, Figures 2.1, 2.2, and 2.3 are given again in this thesis to aid 
in discussing the previous prototype. Figure 2.1 shows a block diagram of the hardware assem- 
bled for at-sea testing of the SANS system. As seen in Figure 2.1, the system relied on an exter- 
nal 386 computer for processing sensor data, a modem connection for transmitting data packets to 
the towing vessel, and a coax cable to receive GPS data from the towed vehicle GPS antenna. The 
output of the A/D converter was being fed to the data-logging computer via an RS-232 connec- 
tion, and the GPS and DGPS receivers were physically located on the towing vessel. The hard- 
ware configuration for this research has been changed considerably, and will be presented in a 


subsequent chapter. 
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Figure 2.1: Towfish Experiment Hardware Configuration 
{Bachmann 95] 
iz 


ote-e 


ODA EP 


‘ 
er eC 
ersateacastacetetenetecenacr 





Shipboard Unit 


GPS Antenna 
BIS Rea waterproofed) 
Differential Revr 


TNC 
7 é TOWFISH 
Electronics Tray 


Pr DLICI U Co 





Lower fin Depth Speed Modem Motion 10 Hz Tattletail Compass 
to house sensor sensor Pak filter data logger 
Depth & Speed (IMU) 


Figure 2.2: SANS and Towfish Components [Bachmann 95] 
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Figure 2.3: SANS Software Objects and Data Flow [Bachmann 95] 


Figure 2.2 presents a photograph of the major components of the previous prototype. Figure 
2.3 shows the SANS (as previously configured) software objects and the types of data passed from 
one to another. In its current configuration, the INS object no longer receives data from the 
sampler object, but rather from processor registers. Again, changes to the SANS software 


configuration will be presented in more detail in a subsequent chapter of this thesis. 


G. SUMMARY 

The above survey has shown that there are many ways to overcome the challenges associated 
with AUV navigation. The choices range from simple dead reckoning to systems which use 
acoustic information from floating or stationary transponders, to complex systems which use 
sophisticated IMUs and GPS receivers combined with Kalman filtering techniques. 

Many AUV missions could be accomplished using an integrated navigation system combining 
GPS and INS. Similar systems in other applications have been demonstrated to have superior GPS 
signal acquisition and reacquisition performance whenever loss of lock occurs. This results in 
improved survivability in hostile environments and smaller navigation errors. This research 
continues an ongoing experimental study pertaining to the development of such a system and the 
associated problems. The current system under evaluation is of small physical size and relatively 
low cost. The IMU selected 1s representative and has limited accuracy, so additional water-speed 
and magnetic heading information is required. This means that accelerometers are used mainly to 
derive low frequency attitude information, and are not utilized for velocity or position estimation 
over long periods. 

The availability of differential GPS in open-ocean tests in Monterey Bay will allow the 
experimental choice of navigation filter gains to accurately assess overall system performance in 
a variety of sea states and for various operational scenarios. Previous research on the prototype 
SANS has produced test results and qualitative error estimates which indicate that submerged 
navigation accuracy comparable to GPS surface navigation is attainable. The research goal of this 
thesis is to refine the error estimates and the hardware configuration to allow more prolonged 
submerged navigation, and develop the SANS into a self contained system capable of being 


internally or externally attached to any AUV and delivering regular, accurate position updates. 
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{. EVALUATION OF THE PROTOTYPE SANS HARDWARE 


A. INTRODUCTION 

At the heart of the prototype SANS are the GPS receiver, the IMU, and other sensor devices. 
The GPS receiver, compass, and water speed sensor have not, up to this point in the course of this 
research, presented any serious problems in accuracy or dependability. However, the IMU out- 
puts, after being fed through signal processing and conditioning circuitry, are suspected sources of 
navigation inaccuracies [Bachmann 95]. 

This chapter does not provide any evaluation of the GPS/DGPS receivers, water speed sensor, 
Or compass since there is no apparent major error contributions from these devices. It does, how- 
ever, provide an investigation into what noise is present in the real-world Systron-Donner 
MotionPak IMU. It also provides an evaluation of the low-pass filtering and conditioning cir- 


cuitry. Finally, it presents an evaluation summary of the prototype SANS hardware. 


B. NOISE CHARACTERISTICS OF THE MOTIONPAK INERTIAL SENSORS UNIT 

The Systron-Donner Model MP-GCCCQAAB-100 “MotionPak” inertial sensor unit consists 
of a cluster of three accelerometers and three “Gyrochip” angular rate sensors. The accelerome- 
ter specifications are shown in Table 3.1. The angular-rate sensor specifications are given in Table 
3. 

With the IMU placed on a stable test bench, polariod photographs were taken of an oscillo- 
scope screen display of each sensor output. Figure 3.1 depicts the respective noise characteristics 
in each of the IMU sensor outputs. This analysis gives hints there is broadband, “white” noise in 
the accelerometer sensors, while there is a 275 Hz sinusoidal signal present in the angular-rate 
sensors. Figures 3.2 and 3.3 depict the results of a power spectrum analysis of the accelerometer 
and angular-rate sensor outputs. Figure 3.2 shows the power spectrum between 8 Hz and 100 
KHz as well as between 8 Hz and 50 Hz. The spectrum analyzer used for these tests has its own 
power spectrum which was sufficiently below the sensor signals by 8 Hz. Thus 8 Hz was chosen 
as the start frequency. As these plots show, with a reference level of -20 dB (the reference level is 
depicted as the top-most horizontal index line), and using the value of 10 db/Div, the signal has a 
maximum value of -60 dB below 50 Hz. The signal energy drops off above this point and 


becomes relatively flat in the vicinity of 100 KHz. Figure 3.2 shows that the noise in the acceler- 
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Table 3.1: MotionPak Accelerometer Sensor Specifications [Systron-Donner 94] 
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Table 3.2: MotionPak Angular-Rate Sensor Specifications [Systron-Donner 94] 
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Figure 3.1: Systron-Donner IMU Sensor Noise Characteristics 
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Ometers is in fact broadband white noise, since it shows a power spectrum with energy across a 
wide range of frequencies. The white noise is most likely thermal and low-level EMI noise. Fig- 
ure 3.3 shows there is more “color” in the angular-rate sensor noise. Mainly, there appears to be 
harmonic noise between 1.5 KHz and 300 KHz and several high-power noise peaks at 100, 200, 
and 275 Hz. The peak at 275 Hz is most likely harmonic noise from the fundamental tuning-fork 
in the rate sensors. [Matthews 95] describes an investigation of signal noise in this same Systron- 
Donner MotionPak IMU. [Matthews 95] found a strong peak at 275 Hz, and attributed this noise 
to the internal tuning-fork oscillator in the sensor itself. 

The accelerometer sensor outputs depicted in Figure 3.1 show a DC bias of -21 mV and 125 
mV respectively. These DC biases are most likely caused by a combination of electronic sensor 
bias and slight tilts in the test bench surface. For the purpose of this evaluation, these DC biases 
are ignored. Furthermore, the software as described in [Bachmann 95], uses bias correction fac- 
tors that essentially negate any sensor bias affects on navigation accuracy. 
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Figure 3.2: Power Spectrum of Accelerometer Sensors 
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Figure 3.3: Power Spectrum of Angular-rate Sensors 


C. THE SIGNAL PROCCESSING AND CONDITIONING CIRCUITRY 


The signal processing and conditioning circuitry, described in detail in [Schubert 95], low-pass 
filters the IMU outputs at a cutoff frequency of 10 Hz using an active, anti-aliasing, two-pole 
Sallen-Key Bessel filter design. This circuit further converts the dual-ended output swing of the 
IMU to a single-ended 0-5 volts. 

Figure 3.4 depicts the frequency response of the low-pass filter. Generally, this analysis con- 
firms that the -3dB frequency is about where it’s supposed to be, and the response does in fact 


rolloff at the —— -40dB/decade. 
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Figure 3.4: Frequency Response of the Low-pass Filter 
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For the SANS application, in order for any sensor noise to not degrade the accuracy or resolu- 


tion of the measured signal, the energy in the filtered signal above 10 Hz must be attenuated down 


below: 
20log plage —72dB 
412 


As seen in Figure 3.4, the low-pass filter is not successfully able to attenuate signal noise 
energy in the stop-band below approximately 1 KHz (the center horizontal index line is at -70 
dB). The results of this filter short-fall can be seen by looking at the power spectrum of the accel- 
erometer and angular-rate sensor outputs after low-pass filtering. Figure 3.5 and Figure 3.6 depict 


the power spectrum of the filtered accelerometer and ee sensors respectively. 
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Figure 3.6: Power Spectrum of the Filtered Angular-rate Sensor Output 
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This short-fall is more apparent in the angular-rate sensor outputs than those of the filtered 
accelerometer outputs. The low-pass filter appears to be doing an acceptable job in attenuating 
the noise energy in the accelerometers. However, the low-pass filter is not fully attenuating the 
noise energy in the angular-rate sensors to a level below the resolution of the least significant bit 
(LSB). Specifically, there are noise peaks at 30, 100, and 275 Hz that reach -60dB. Clearly, these 
high energy noise peaks are affecting the LSB. By noticing that these noise peaks represent a 


12dB over-power above the required noise floor, and solving the following equation for N: 


log, = -12dB 
“2 
N = 1,99 


it’s apparent that this noise energy is actually affecting the two least significant bits of the mea- 
sured angular-rate signal. 

During system development, it was discovered that the x-angular-rate signal being input to the 
A/D had apparent slow growth behavior. Figure 3.7 shows the results of sampling the x-angular- 
Tate sensor once every 60 seconds for a period of 4 hours after starting the SANS from an initially 


“cold state. 
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The system hardware configuration used to collect the data shown in Figure 3.7 included the 
the LPF circuitry described in [Bachmann 95]. This growth in the signal turned out to be a signif- 
icant discovery possibly explaining some of the inaccuracy the SANS had been experiencing dur- 
ing previous testing. It was at first decided that this growth was due to heating effects in the IMU. 
However, after replacing the LPF circuitry with a newly designed circuit using commercially pro- 
vided filters (this new filter circuit is described later in Chapter IV), the problem went away. 
Therefore, the growth error in the x-angular-rate signal was attributed to heating effects on the cir- 
cuit components conditioning the x-angular-rate signal in the LPF circuitry used in the prototype 


SANS. 


D. SUMMARY 

This analysis lends further explanation to the results obtained in [McGhee 95], which shows 
the angular-rate output fluctuating as much as 4.88 mV; the three least significant bits of the 12-bit 
measured signal. Some of this fluctuation could be attributed to any of the causes stated in 
[McGhee 95], but this analysis gives qualitative evidence that the two least significant bits of the 


measured angular-rate data were in error due to sensor noise. 
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IV. SYSTEM HARDWARE CONFIGURATION 


A. INTRODUCTION 

Figure 4.1 presents a block diagram for the hardware making up the redesigned SANS. Figure 
4.2 presents a photograph of the SANS components fully assembled into their testing configura- 
tion. The project box in which the components are currently mounted is not intended to be the 
final resting place for these components. A more permanent, water-tight, streamlined housing is 
currently in development. In fact, by using this particular off-the-shelf project box, and by itera- 
tively installing, testing, changing, and then reinstalling the components, an optimum space-effi- 
cient configuration has been achieved, which, in turn, has driven the design of the final housing. 

This configuration is significantly different from that presented in [Bachmann 95] for the pre- 
vious prototype. Mainly, the SANS components are no longer separated; all its components are 
physically located in one, self-contained package. In fact, when joined with its accompanying 
power source (a 12 VDC battery), it 1s capable of being strapped-down to a testing turntable, or 
inserted into the towfish (with slight towfish modifications) described in [Bachmann 95]. In its 
current configuration, the SANS has its processor and GPS/DGPS components “onboard,” thus 
no longer requiring the transfer of sensor data via modem to an external processor or GPS/DGPS 
receiver as was shown in [Bachmann 95]. In order to maintain the ability for human monitoring 
and interaction during the course of an experiment, the SANS’s processor is linked with an exter- 
nal processor via a DOS TCP/IP network environment. This external processor’s only function is 
to maintain a remote control session with the SANS processor and receive its attitude and position 
updates. Contrary to the original SANS proof of concept design presented in [Bachmann 95], the 
SANS now maintains the capability to on-board-process its own data in order to maintain its gen- 
eral capability to interface with any other higher-level processor (via a network), regardless of the 


application. 


B. HARDWARE DESCRIPTION 


1. Computer 
The on-board processor is an Extremely Small Package (E.S.P.) Cyrix 486SLC DX2 50 MHz 
computer. This computer, pictured in Figure 4.3, is specifically designed to offer off-the-shelf 


PC-compatible solutions in space and/or power constrained environments. Together, the E.S.P. 
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Figure 4.1: Block Diagram of the Redesigned SANS Hardware Configuration 


Processor Module and its accompanying modules provide a small, low-power system that does 
not sacrifice system performance compared to a standard, desk-top type system [MAXUS 95]. 
This particular E.S.P computer possesses a total of eight modules which perform various system 


tasks. A brief description of the tasks these various modules perform in the SANS is as follows: 


a. 486SLC DX2 CPU Module 

Besides providing the processing capability, the CPU Module provides the interface for a 
standard keyboard, the Flash PROM containing the system BIOS, and memory and bus controller 
logic [MAXUS 95]. 
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Figure 4.2: SANS Hardware Configuration 





b. DC-DC Power Module 


Provides for all the system power requirements up to a maximum 35W total output. It 
accepts an unregulated 12 VDC and provides the required +5, +12, -12, and -28 VDC to power 
various system components and optional peripherals (1.e., external floppy/hard drive). [MAXUS 


275) 


c. VGA Adapter Module 


Provides the interface to operate an external VGA monitor. 


d. PC I/O Module 


Provides for 2-Serial ports and 1-Parallel 1/O port. 


e. PCMCIA Module 


Provides two type-II] PCMCIA sockets which conform to PCMCIA Release 2.01 standard 
[MAXUS 95]. These two ports can be used for a variety of compatible devices (1.e., Ethernet 
Adapter, Modem, GPS Receiver, etc.). For instance, this module can accept SRAM cards which 
can contain bootable system files or simply be used to provide storage media. This module was 
included in the current design to provide additional secondary storage in the form of PCMCIA 


SRAM cards, as well as to enable future expandability. 
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f. Ethernet Module 


Provides the SANS with an external ethernet interface. 


g. Analog to Digital (A/D) Module 


Provides 8 differential or 16 single-ended input channels at 12-bit resolution. It features a 
single-channel maximum sampling rate of 333 KHz, and an input range from +/- 1.25mV to +/- 
1OV [MAXUS 95]. The A/D module provides a 34-pin external connector (J3) to which develop- 
ers can connect their input signals. In its current configuration, the A/D module samples only 8 of 
the available 16 single-ended channels. Table 4.1 shows the current pinout of connector J3 on 


the A/D connector board. 
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h. DRAM Module 


Provides for high-speed (/0ns) memory storage available in 2, 4, 6, 8,or 16MB capacities 
({MAXUS 95]. This module is to the E.S.P. as a hard disk is to a standard desk-top PC. 


2. Inertial Measuring Unit 


The inertial navigation component of the SANS is provided by a Systron-Donner Model MP- 
GCCCQAAB-100 “‘MotionPak”’ inertial sensing unit, pictured in Figure 4.4. This self-contained 
unit provides analog measurements in three orthogonal axes of both acceleration and angular 
velocity. It consists of a cluster of three accelerometers and three “Gyrochip” angular rate sen- 
sors. General specifications are shown in Table 4.1. Accelerometer specifications and angular rate 


specifications are shown in Table 3.1 and Table 3.2 respectively. 





Figure 4.4: Systron-Donner Inertial Measuring Unit [Bachmann 95] 
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Table 4.2: MotionPak General Specifications [Sytron-Donner 94] 























3. GPS/DGPS Receiver Pair 


The GPS/DGPS receiver used is the ONCORE 8-channel receiver which incorporates an 
imbedded DGPS capability [Oncore 95]. The receiver is capable of tracking up to eight satellites 
simultaneously. It can provide position accuracy of better than 25 meters Spherical Error Proba- 
ble (SEP) without Selective Availability (SA) and 100 meters (SEP) with SA. Typical Time-To- 
First-Fix (TTFF) is 18 seconds with a typical reacquisition time of 2.5 seconds [Oncore 95]. This 
receiver meets or exceeds the capabilities of the receiver described in [Norton 94], which demon- 
strated that under normal operating conditions, a receiver of this kind, is capable of meeting the 
accuracy and time requirements of the SANS project. [Norton 94] also demonstrated that a 
receiver with these qualities will perform well when using an antenna that is located on or near the 
sea Surface as is necessary during a clandestine mission. Figure 4.5 shows the ONCORE GPS/ 


DGPS receiver used in the SANS project. 





Figure 4.5: ONCORE GPS/DGPS Receiver 
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4. Low-Pass Filters 

Based on the analysis given in Chapter III, the 2-pole Bessel filters were replaced with new fil- 
ters which offer a faster “rolloff’ in the stopband (now 80 dB/Dec). Each of the six IMU outputs 
is filtered by a 4-Pole, active, anti-aliasing low-pass Butterworth filter with a bandwidth of 10 Hz. 
The low-pass filters are model DP74 and are packaged in a standard 16-pin dual in-line package 
(DIP) [Frequency Devices 96]. These filters feature low-harmonic distortion, come factory tuned 
to a user-specified corner frequency, require no external components or adjustments, and operate 
with a dynamic input voltage range from non-critical +/- 5V to +/-18V power supplies [Frequency 
Devices 96]. To implement these filters into the SANS, a double-sided printed circuit board 
(PCB), shown in Figure 4.6, was designed and machined to receive all six filter DIPs as well as 
three quad op-amp LM324 DIPs configured as voltage-followers to provide input and output cir- 
cult protection. Figure 4.7 shows the schematic diagram for one channel in this low-pass filter 


circuitry. 





Figure 4.6: The Double-Sided Low-Pass Filter PCB 


Vin 


Out to A/D 





In From IMU /4 LM324 1/4 LM324 





+15V 712 y 


GN 


Figure 4.7: Schematic Diagram for One Channel of the Low-pass Filter Circuitry 
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5. DC-DC Converter 

To provide for the requisite +/-15 VDC, a DATEL model BWR-15/330-D12 DC-DC Con- 
verter is used to convert the unregulated 12 VDC battery input to regulated +/-15 VDC to power 
the low-pass filter circuits as well as the IMU. This converter features over-current and short-cir- 
Cuit protection, a compact form-factor, high reliability, a minimum efficiency of 82%, and 
employs switching regulator technology, which minimizes heat generation and current usage 
[DATEL 95]. Though the DC-DC converter ensures a low noise/ripple in the output signal, the 
converter is augmented with additional capacitors in parallel with both the input and output pins 


of the device. Figure 4.8 shows the use of these capacitors in the DC-DC converter circuitry. 


Cl: 0.1 uF Electrolytic Capacitor 
C2: 100 uF Electrolytic Capacitor 





-15 V Out 


Figure 4.8: DC-DC Converter Circuit 


The value of Cl was selected so as to filter any high-frequency radio frequencies (RF) that 
may get inducted into the circuit from surrounding components, and the value of C2 was selected 
so as to filter any high-frequency switching noise from the converter itself (the converter switches 
at 165 KHz) [DATEL 95]. This DC-DC Converter is physically mounted on the same PCB on 
which the low-pass filters are mounted. This configuration allows the use of PCB “tracks” to sup- 
ply the filter circuitry with +/-ISVDC. This regulated +/-15VDC as well as IMU sensor signals 
are routed to and from the PCB via a DB25. 


6. Ribbon Cable 


Physically connecting the IMU, Low-pass Filters/DC-DC Converter PCB, the Analog-Digital 
Converter, input power, water speed sensor, and depth sensor, is a 25-strand flat ribbon cable. 
This cable enables all system components to be easily interconnected. Table 4.2 gives the pinout 


of this 25-strand ribbon cable as well as all DB25 connectors used in the system.. 
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Table 4.3: Pinout of the Ribbon Cable and DB25 Connectors 
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7. Compass 


The compass used in the SANS project is a Precision Navigation model TCM2 Electronic 
Compass Module. This compass does not employ the mechanical gimbal technology utilized in 
the compass described in [Bachmann 95], but rather employs a three-axis magnetometer and a 
high-performance two-axis tilt sensor in a small form-factor [TCM2 95]. The TCM2 compass 
provides readings of not only heading, but also pitch, role, and surrounding magnetic field 
strength. The TCM2 provides greater accuracy by calibrating (performed by the user) for distor- 
tion fields in all tilt orientations, providing an alarm when local magnetic anomalies are present, 


and giving out-of-range warnings when the unit is being tilted too far [TCM2 95]. 


8. Other Components 


The water speed sensor and the depth sensor are those described in [Bachmann 95] and there- 
fore not depicted in Figure 4.2. The GPS antenna shown in Figure 4.2 is an active antenna, which 
was selected for its performance and low-profile. Because the E.S.P. Ethernet module’s output 
media type is AUI, a standard AUI-to-BNC media converter is employed to allow the use of dura- 
ble RG-58 coax cable to span the roughly 100m distance required while pulling the towfish 
behind a towing vessel. The GPS/DGPS Interface box is nothing more than an adapter to inter- 


face the GPS receiver signal with the serial COM? port of the E.S.P. computer. 


C. SUMMARY 


The SANS design described in this chapter is significantly different from that described in 
[Bachmann 95]. The processing capability, along with the GPS/DGPS receiver, is now on-board 
the SANS, making it completely self-contained with its only external link being that of a DOS 
ethernet environment to a remote PC. The IMU sensor data, after low-pass filtering, along with 
water speed and depth data, are converted from analog to digital with 12-bit resolution, and then 
joined with GPS data in the on-board computer which computes updated attitude and position 
information to be exported over an ethernet socket. The hardware for this version of the SANS 
was chosen to comply with the requirements set forth in [Kwak 93]. Though there are many pos- 
sible choices of hardware for each of the components in Figure 4.2, trade-offs between accuracy, 


size, power requirements, and cost must be considered. As further advances in miniaturization are 
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made, accuracy will continue to increase while price and size decrease, thus making it easier to 


meet the challenges of the SANS baseline requirements. 
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V. SOFTWARE DEVELOPMENT 


A. INTRODUCTION 


The purpose of the SANS software remains the same as that described in [Bachmann 95]: “to 
utilize IMU, heading, and water-speed information to implement an INS, and then integrate this 
with GPS information into a single-system which can produce continually accurate navigational 
information in real time.” This chapter will not re-introduce the software already adequately pre- 
sented in [Bachmann 95], but rather will present only those changes, additions and updates, to the 


software described for use on the previous prototype SANS. 


B. SOFTWARE DESCRIPTION 


This implementation continues to use a majority of the software designed by [Bachmann 95}, 
and for the most part, unchanged. However, the changes in the hardware architecture have driven 
subsequent changes to the software design to enable its use in the current SANS. Figure 5.1 
shows the software objects and data flow of the current SANS. Though already previously stated, 
these hardware changes, along with the subsequent software changes and additions, are again pre- 


sented in detail as follows: 


1. Compass Data 


In the SANS described by [Bachmann 95], the compass data was included in the packets 
received via modem from the towfish. This compass data was then parsed out of this packet for 
use. This Xmodem packet code is no longer required and has been removed. In the current ver- 
sion of the SANS, the compass data is received via the COM2 serial port, and thus requires code 
to communicate with the serial port, as well as code to check the “checksum” and header of each 
compass message received. This change was easily implemented by simply cutting, pasting, and 


altering previous com port and checksum code. This code is provided in Appendix A. 


2. GPS Data 


The code required to process GPS information is only slightly different from that described by 
[Bachmann 95]. The only changes required were driven by the use of an 8-channel receiver vice 
the 6-channel GPS receiver used previously. The 8-channel receiver sends a longer message, thus 


the only changes were to adjust the message length and the location of the checksum character in 
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the GPS message. 

The code was also modified to include a differential check. Before the code recognizes a GPS 
message as being valid, the message must pass three conditions; 1) A checksum check, 2) The fix 
must be based on at least 4 satellites, and 3) The differential bit in the message must be set (.e., 


the fix must have the differential correction calculated into it). This updated code is provided in 
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Figure 5.1: SANS Software Objects and Data Flow 


3. Inertial Sensor Data 


Like the compass data, the inertial sensor data was previously included in the packets received 
via modem from the towfish. Currently however, the filtered inertial data is input directly to the 
A/D converter module in the on-board processor. Therefore, there is additional code running in 


the SANS software system which operates the A/D converter module, as well as buffers this data. 


a. A2D 


The A/D module, when delivered, came with C source code to run a demo of the module. 
This code was modified and converted to C++ to fit the SANS application and its object oriented 
design. The A2D class provides all the requisite software operation of the A/D module in the 
E.S.P. computer. The A/D module is completely controlled through software. Primary to this 
control is the manipulation of the A2D Control Register and the A2D Status Register. These reg- 
isters are manipulated by writing to and reading from specific memory addresses. Specifically, 
the A2D Control Register is at address 0x 108, and the A2D Status register is at 0x102. The A2D 
class is designed such that the user maintains some degree of flexibility. For instance, the user can 
choose between one of two base addresses, 0x100 or 0x300, from which these important A2D 
registers are then created by adding an offset (O8h and 02h respectively) to this base address. 

Though the A2D class has many member functions, the SANS software only uses a few in 
accomplishing its mission. All those member functions not directly utilized in this particular 
application are useful to troubleshoot problems with the A2D module, or allow a wide range of 
options to tailor its use to a particular application. The class code is well commented, and easily 
stands without any further discussion. However, for the benefit of the reader, the following gen- 


eral discussion of how the A2D module works in the SANS application is deemed beneficial. 


The A2D, as stated in Chapter IV, provides 12 bits of resolution, or 2'* = 4096 discrete 
quantization levels. There are two modes to employ the A2D module: differential mode, or sin- 
gle-ended mode. The SANS application employs the A2D in the single-ended mode of operation. 
In this mode, the A2D is able to sample the dual-ended swing of the IMU sensor signals, and rep- 
resent these voltages as a digital value in the range 0 - 4095. A general A2D conversion table is 
provided as Table 5.1 to further illustrate how the sensor voltages are mapped over to their digital 


equivalents. 
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Table 5.1: A2D DC-to-Digital Conversion Mapping 







When an A2D object is instantiated, the class constructor (see Appendix A, A2D.cpp) sets 
several default data member values, and then reads the A2D configuration file A2D.cfg. This 
configuration file provides a simple manner in which a user can change the way the A2D module 
operates without having to re-compile the source code. When the function readConfigFile(Q) is 
called, it reads the A2D.cfg file one line at a time and loads those respective variables described 
on each line of this file with those values found on each line. This prepares the software for ini- 
tializing the A2D hardware. The constructor initializes the system addresses to setup for A2D 
operation, then initializes the A2D hardware using those variables that were loaded upon reading 
the configuration file. The A2D object gets instantiated when a Sampler object gets instantiated 
as the A2D object “a2d1” is a private data member of the Sampler class. 

The A2D module gets set into operation by a call to initsampler(). This sampler class 
member function executes a sequence of function calls to A2D class member functions which set 
the A2D module into operation. Mainly, they program the sequencer to tell it which channels to 
sample in which order, reset the A2D First-In-First-Out (FIFO) to enable it to receive data, and 
then toggle the trigger bit in the A2D Control Register from a low to a high which starts the A2D 
into operation. 

During SANS operation, the Sampler class member function readSamples() is called 
repeatedly to retrieve inertial data from the A2D FIFO. It first checks to ensure that the FIFO is 
not FULL. If the FIFO ever gets filled without being immediately emptied, it will continue to 
push data into the FIFO. Since there is no room for additional data, all samples from that point on 
will be lost. So, it is evident that preventing the FIFO from overflowing is paramount to SANS 


operation. If the this check is ever true, the SANS software will terminate execution. To prevent 
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FIFO overflow, one need only be mindful of the rate at which the A2D is sampling its inputs and 
be sure to empty out the FIFO at the same rate or faster. If the FIFO does have data in it, this data 
is emptied from the FIFO and stored in a doubly-subscripted array with 8 rows and 1000 columns 
to coincide with storing up to 1000, 8 channel samples of sensor data. Figure 5.2 presents a 


model of this doubly-subscripted array and how it stores the data. 





Sample Number/Array Index 


Figure 5.2: Model of the A2D Sample Array 
As the samples are being emptied from the FIFO, the variable “timeCounter” is incre- 
mented once for every 8 samples that are pulled from the FIFO. This variable is then multiplied 
by the sample period to calculate the “deltaT”’ or the time between adjacent samples. The reason- 
ing behind using this type of data structure to temporarily store the data is to enable access to a 
history of samples in order to employ a digital filtering scheme. In the case of the SANS, it 
employs a very simple form of low-pass filtering by averaging over all the samples received since 


the last sample was taken from this array. 


C. SUMMARY 


All software additions and updates to the SANS software were made in keeping with object- 
oriented paradigms. The software was written in Borland 3.1, C++ for DOS. 

Since the publication of [Bachmann 95], there have been many significant as well as minor 
changes/updates to the SANS software. The compass data is now received from a serial port vice 
being received via an Xmodem packet. The GPS data is still received from a serial port, but the 


GPS receiver is now an 8-channel receiver instead of a 6-channel. The inertial, water speed, and 
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depth data is no longer being received via an Xmodem packet, but rather is being input directly 
into the A/D module in the E.S.P. computer. Consequently, all the Xmodem packet code is no 
longer used in the SANS software system. All these hardware changes have driven software 
changes that have propagated throughout the software. For this reason, a complete copy of all 
SANS software is given as Appendix A. A further discussion of those software updates incorpo- 
rated into the SANS between the publication of [Bachmann 95] and this thesis, is presented in 
[Bachmann 96}. 
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VI. SYSTEM TESTING 


A. INTRODUCTION 


This chapter presents the test method and the experimental results of testing used to determine 
the functionality and accuracy of the SANS. Bench testing was performed to ensure the entire 
system was functioning properly in its current state. The system was then tested by conducting 
tilt-table tests in order to verify the operation of the Inertial Measurement Unit and its associated 
filter software. It was further tested by conducting a static GPS test to test the operation of the 
positioning capabilities of the SANS. Lastly, it was given a static heading test to ensure proper 
operation of the Kalman filter in determining heading. 

Figure 6.1 presents a data flow diagram of the SANS navigation filter. This diagram is pre- 
sented at this point to provide a basis for discussion of the following test results. The reader is 
referred to [Bachmann 95, 96], and [McGhee 95] for an in-depth discussion of this filter. This 
twelve-state velocity-aided navigation filter is implemented in the SANS software provided as 


Appendix A and Appendix B. 


B. IMU TESTS 


The purpose of these tests was to qualitatively ensure that the SANS was able to accurately 
track changes in attitude. Paramount to getting the SANS to do this, is to find a suitable value for 
the gain K1(shown in Figure 6.1), the biasWght, sampleWght (all in INS.CPP), and x/yAc- 
celScale factors (¢n LOCATION.H). The results presented in this chapter are for the pitch axis 
only. A similar process still remains to be done for the roll axis. 

With the SANS mounted to the tilt-table described in [Bachmann 96], a series of pitch tests 
were conducted. For these tests, the IMU outputs were sampled at a rate of 40 Hz. During test- 
ing, it was noticed that the SANS produced update rates between 6-10 Hz. 

Tuning data for the SANS was obtained by moving the SANS unit through pitch excursions 
within a 45 degree range. The attitude as determined by the SANS was then plotted and compared 
with the actual motion of the unit. Through this comparison, it was possible to determine an initial 
scale factor. This process was repeated several times until the attitude determined by the filter 
‘matched’ the true motion of the unit. From this analysis, a rate sensor scale factor of 4.1 enabled 


the filter to register a pitch of 45 degrees when the SANS was pitched to that angle. 
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Figure 6.1: SANS Navigation Filter 

In order to tune the filter for accurate operation, the rate sensor bias value must first be deter- 
mined. To do this, the gain value K1 is set to zero in order to prevent accelerometer data from 
getting to the first integrator. Without the accelerometer data, only the high frequency data from 
the angular-rate sensors is input to the first integrator with the estimated bias. Taking the com- 
manded tlt-table angles to be truth then, any errors in attitude can be attributed to the bias and 
scale factor. 

Having determined the initial scale factor to be 4.1 and setting K1=0.0, a series of pitch tests 
were conducted in order to determine the correct bias weights. In order to obtain a starting point 
for this analysis, and based on similar testing discussed in [Bachmann 95], an initial bias value 
(biasW ght) of 0.999 was chosen for the first pitch test. Figure 6.2 shows the results of the first 45 


degree pitch excursion with K1 = 0.0, bias value = (0.999, and the scale factor set to 4.1. 
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Time (seconds) 
Figure 6.2: 45 Degree Pitch Excursion w/K1=0, biasWght=.999, scale factor= 4.1 

The shape of the plot in Figure 6.2 is determined by the value of a “virtual’ time constant. For 

the sake of this discussion, this time constant 1s T. The method in which the SANS software 


determines the rate bias value can be essentially modeled as a low-pass filter. Specifically, the 
SANS software figures this rate bias according to the model depicted in Figure 6.3. This figure 


describes a linear system where the accumulating rate bias is subtracted from each new angular- 
rate value, multiplied by the sampleWght ((Af) /T), which is (1 - biasWght), and then this 


value 1s added to the rate bias sum. 


rate bias 





Figure 6.3: Model of Rate Bias Low-pass Filter 
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From linear system theory, in the s-domain, it can be shown that the transfer function of this 
system is: 


l 
1+ts 





Assuming the 10 degree/second pitch excursion to be a unit step function, the unit step 


response of this system is: 


tn = (1 ars 


By taking the first derivative, it can be shown that the bias filter output slope is 1/t. Using the 
relation that the sampleWght (.001 for the first test) is equal to the ratio of the time between sam- 
ples (at a sample rate of 7 Hz, this gives a period of 0.142) and T, it can be shown that, for the first 
test depicted in Figure 6.2, t is approximately 142 seconds. That is, 


0.001 = - = “tot = 142 sec 


Given the known angular-rate of the pitch excursion was 10 degrees/second, the bias filter output 


slope can then be calculated as: 


10 
Tie = 0.07 (deg/sec)/sec 


From the plot, it appears to have taken about 4.5 seconds to complete the 45 degree angular-rate 
excursion. Multiplying the filter slope by the 4.5 seconds yields a resulting bias estimate of 
approximately 0.3 deg/sec. Again, from the plot, the slope of the curve after it reached 45 deg, 
but before it started its return to zero, agrees with the calculations. 

In short, the filter is behaving just as it should. During the course of the excursion to 45 
degrees, the bias filter accumulates a rate bias of 0.3 deg/sec. Since the filter time constant is only 
142 seconds, and the SANS only underwent a 4.5 second excursion, it did not have time to correct 
for this bias. Additionally, since the bias 1s subtracted from each new sample, the result is the 
negative sloping portion of the curve while the SANS is at 45 degrees pitch. 

Obviously, the goal of this analysis is to minimize any accumulated rate bias while the SANS 
is undergoing pitch or roll excursions. To accomplish this, the value of the time constant T needs 
to be long enough to minimize the bias filter slope, which in turn, minimizes the accumulated rate 


bias. 
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Figure 6.4: 45 Degree Pitch Excursion w/K1=0, biasWght=.9999, scale factor= 4.1 


Figure 6.4 shows the test results after increasing the filter time constant by a factor of 10. As 
shown in the plot, the pitch values sloped off less by about a factor of 10 (as expected) during a 
pitch excursion to 45 degrees. The decreasing curve leading up to the 45 degree pitch is due to 
not initializing the bias filter with enough samples. For these tests, the filter was only initialized 
with 100 samples, which represents about 15 seconds. However, since the time constant is much 
larger, the filter should be initialized over more samples. Ideally, the filter should be initialized for 
t seconds. The plot still shows some bias effect as well as the effects of undersampling (or slow 
update rate). Here, the navigation filter gets an update just before the tilt-table stops, but the filter 
still thinks it has a rate bias and applies it to the next sample, which occurs after the tilt-table has 
stopped. 

Figure 6.5 shows the result of another similar test, only with K1 = 0.1, biasWght = .9999, 
sampleW ght = .0001, and xAccelScale = 4.1. This test re-introduces the low-frequency data 
(accelometer data) into the integrator. This plot shows yet more accurate performance, but dis- 
plays an “overshoot” in both directions of the excursion. This overshoot can be alleviated by 


adjusting the scale factor. 
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Figure 6.5: 45 Degree Pitch Excursion w/K1=0.1, biasWght=.9999, scale factor= 4.1 


Figure 6.6 shows the result of another similar test, only with K1 = 0.1, biasWght = .9999, 
sampleWeght = .0001, and an adjusted scale factor, xAccelScale = 3.895. This plot shows an 
undershoot, indicating the scale factor was too low. Additionally, it still shows a small amount of 
sampling effect. This can only be corrected by getting the navigation filter to run faster. This can 


be easily accomplished by adding a math co-processor to the SANS computer. 
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Figure 6.6: 45 Degree Pitch Excursion w/K1=0.1, biasWeght=.9999, scale factor= 3.895 
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C. STATIC GPS TEST 


The purpose of this test was to ensure proper functioning of the DGPS and the Kalman filter in 
calculating position. With the SANS stationary, operating, and receiving differentially corrected 
GPS fixes, position updates were recorded over a 30 minute period at a rate of 10 Hz. Figure 6.7 
confirms proper operation of the DGPS hardware and the Kalman filter software. The position 


updates show an oscillating behavior roughly centered about the origin. 


Y Grid Position (feet) 





X Grid Position (feet) 
Figure 6.7: Static GPS Test Results 


The results shown in Figure 6.7 are well within the 10 meter requirements outlined in [Kwak 
93]. By looking at a plot of the x position, it was determined that the greatest drifts shown in Fig- 
ure 6.7 occurred early into the test, and as time went on, these drifts got smaller. This plot shows 
how the SANS Kalman filter “learns” as time progresses. In fact, this drift becomes smaller and 
smaller due to the accumulation of apparent current. At the beginning of the test, the apparent 
current is small. Any error in location is attributed to apparent current, so as time goes on, the 
magnitude of the apparent current grows. Because the apparent current velocity is added to the 
North & East velocity, the difference between the INS determined position and the GPS deter- 
mined position decreases with time. Figure 6.8 shows a plot of the magnitude of the apparent cur- 
rent during the same test as that depicted in Figure 6.7. This plot shows that the apparent current 
Starts out small, but quickly adjusts to a randomly varying value reflecting the accumulation of all 


system errors. 
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Figure 6.8: Apparent Current 


D. STATIC HEADING TEST 


The purpose of this test was to ensure that the heading calculated by the Kalman filter was in 
fact continuous in behavior. That is, a rotation through North in the clockwise direction should not 
cause the heading value to go back to zero, but rather continue to increase. With the SANS sta- 
tionary and running, the compass was rotated several complete rotations in both the clockwise 
and counterclockwise directions. As Figure 6.9 shows, the heading value is in fact continuous in 


nature as the plot of heading does not show any discontinuity at the zero degree crossing point. 
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Figure 6.9: Static Heading Test Results 


E. SUMMARY 

This chapter provides an explanation and results analysis of the experimental tests performed 
on the SANS to determine its proper functioning under the newly designed architecture. 

Dynamic tilt-table tests were performed in order to “tune” the software scale factors, bias val- 
ues, and gain factor K1 used in the Kalman filter. These test also confirmed the proper operation 
of the SANS in general. 

The DGPS and the associated software was tested by conducting a static test during which the 
SANS remained stationary while getting position fixes. This test confirmed the proper operation 
of the DGPS hardware and software. The behavior of the results also indicate that the SANS soft- 
ware Kalman filter is properly attributing its navigation errors to apparent current, as expected. 

The compass and associated software was tested by analyzing the heading output of the SANS 
while the compass was rotated through several complete rotations. The results show a continuous 
heading output from the SANS, that is, there are no discontinuous “jumps” as the compass rotates 
through North. 

Though extensive testing of the SANS still remains, initial testing indicates qualitatively that 
the SANS does function properly. Based on observations during this testing, the SANS shows an 


improved performance over the previous proof of concept prototype. Mainly, the SANS now pro- 


51 


vides an improved update rate of up to 10 Hz in comparison to the 5 Hz of the prototype. This 
increase in update rate has reduced the effects of undersampling experienced with the prototype 
and explained in [Bachmann 95]. Figure 6.6 provides insight to conclude the SANS is still suffer- 
ing from some effects of undersampling. This effect will most likely be alleviated with the 


planned addition of a math co-processor to the E.S.P. CPU Module. 
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VII. CONCLUSIONS AND RECOMMENDATIONS FOR FUTURE WORK 


A. CONCLUSIONS 


The research issues addressed by this thesis were 1) Evaluate the hardware architecture of the 
prototype SANS, 2) Develop a hardware configuration which will enable the SANS to be housed 
in one small, self-contained package, and 3) Evaluate the feasibility of an AUV accurately navi- 
gating from point to point in open-ocean transit using this hardware configuration. 

The work conducted in addressing the first of these questions revealed several sources of nav- 
igation inaccuracy. The analog low-pass filter circuitry used in the prototype SANS was not ade- 
quately attenuating the angular-rate sensor noise below the resolution of the least significant bit of 
the 12-bit A/D, and was over-designed to require high current, thus generated a great deal of heat. 
It was also discovered that circuit components were being adversely affected by heat. Specifi- 
cally, the x-angular-rate signal displayed a significant growth behavior over time as the circuit 
heated up. Both of these problems were alleviated by employing higher quality, commercially 
available analog filters, and also employing a switching DC-DC converter in place of the linear 
regulators employed in the prototype SANS. All this circuitry was designed into a small double- 
sided PCB. 

In addressing the second of these questions, an emphasis on making the SANS integrated and 
self-contained produced a configuration in which all SANS components are physically packaged 
in a project box measuring 17.5” X 6.125” X 3.0”. In the prototype SANS, the IMU, compass, 
GPS antenna, and water speed/depth sensors were physically separated from the computer and 
DGPS receiver. The newly designed SANS no longer requires the use of the data logging com- 
puter (which did the A/D conversion and assembled data packets) or the Motorola modems. The 
SANS now employs a E.S.P. 486 DX2 50 mini computer with an internal 12-bit A/D converter, 
DC-DC converter module (enables computer to be powered from a 12 Volt battery), Ethernet 
module (enables SANS to be networked in a client/server environment), PCMCIA module (pro- 
vides SANS with expandability), and memory storage modules. From observing the SANS oper- 
ation during bench testing, it is evident the system runs faster (gives faster update rates) since it 1s 
no longer hindered by the “bottleneck” induced by the Xmodem transfer of data employed by the 
prototype SANS. 
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Though the last research question was not directly addressed in detailed at-sea testing, those 
tests that were conducted for this thesis qualitatively indicate that the newly designed hardware 
configuration provides a higher level of performance to the SANS. From this result, it can be 
extrapolated that the feasibility of an AUV accurately navigating during open-ocean transit, is at 
least as high as that achieved with the prototype SANS. It is likely that the newly designed SANS 


will demonstrate increased accuracy in at-sea trials. 


B. RECOMMENDATIONS FOR FUTURE WORK 


There are many ways to build on the foundation this and related research has established. In 
addition to comprehensive at-sea testing, and since this project research is dynamic in nature, 
there are several other areas that remain to be addressed as they pertain to the SANS. Before any 
worthwhile at-sea testing can be accomplished, further tilt-table/bench testing needs to be con- 
ducted. In order to optimize the performance of the Kalman filter used for inertial navigation, 
testing needs to be done in order to better optimize the gains in the Kalman filter. By analyzing 
the data from varied accelerometer and angular-rate tests, one can better choose what these gain 
values need to be. 

An issue directly related to testing is that of post-processing. The post-processing code writ- 
ten to run on the previous prototype SANS should be updated/changed to enable its use on the 
newly designed SANS. Through post-processing test data, one can more easily run and re-run the 
tests in the goal of more easily optimizing the Kalman filter gains. The current version of the 
SANS software does not save the “raw” sensor data. This should be made possible. With the 
help of post-processing code, one needs only run a particular tilt/bench/cart test one time, and 
then take the raw data back, analyze it, adjust gains and re-run the test in order to optimize gains. 

There are hardware issues that remain and are areas for future work. The SANS uses a paddle- 
wheel type water speed sensor. This is a rather crude and inaccurate sensor, and should be 
upgraded to one which can deliver reliable data at the slower velocities under which the towfish 
and Phoenix AUV operate. Additionally, in order to conduct future at-sea testing, the DGPS 
antenna currently used on the SANS will have to be upgraded. In previous sea trials, the towfish 
would become fowled by seaweed. Under these conditions, the current DGPS antenna will not 
survive. Due to difficulties in acquiring software drivers for the Ethernet module, the network 


connection between the SANS and an external PC was not established during the course of this 


54 


thesis. In order to conduct at-sea testing, this network connection is a must, so work still remains 
in bringing this aspect of the SANS to fruition. 

Lastly, the final step in this research will be incorporation of the SANS into the NPS AUV. In 
this work, there should not be any significant changes to the hardware. However, there will be 
requirements in interfacing this hardware with the operating system running the NPS AUV. It 
may be beneficial from both a space-saving aspect as well as from the aspect of having more com- 
puting power, to transfer all software operation over to the Sun Voyager workstation which cur- 
rently accomplishes mission control for the NPS AUV. In this case, the issues of serial port 
communications, and A/D conversion will have to be readdressed before this can be successful. 
On the other hand, there is a distinct advantage to leaving the SANS as is and simply integrating 
it into the NPS AUV as another client in its established client/server environment. A detailed 


study of these alternatives will be required before the best solution can be developed. 
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APPENDIX A: Real Time Navigation Source Code(C++) 


A. TOWTYPES.H 


#ifndef _— TOETYPES_H 
#define _ TOETYPES_H 


mime lude <stdio.h> 

#include <dos.h> 

#include <time.h> 

#include “globals.h" 7/ Types used by serial communications software 
#define GPSBLOCKSIZE 76// Size of Motorola @@Ea position message 
#define COMPSIZE 60 // Max size of compass message 

#define biasNumber 10 // Number of packets used to calculate initial bias 
#define ONE_G 32.2185// One g in feet per second 

#define GRAVITY 32.2185 // In feet per second 

moeerane TicksToSecs(x) ((double) ((10 * x) / 182)) 

typedef char ONEBYTE; 

typedef short TWOBYTE; 

typedef long FOURBYTE; 

typedef unsigned char UNSIGNED_ONEBYTE; 

typedef unsigned short UNSIGNED_TWOBYTE; 

typedef unsigned long UNSIGNED_FOURBYTE; 


// Holds lat/long expressed in milseconds 
struct latLongMilSec { 

long latitude; 

long longitude; 
Is 


// Holds a latitude or longitude expressed in hours minutes and degrees 
Strider © GEODETIC { 


TWOBYTE degrees; 
UNSIGNED_TWOBYTE minutes; 
double seconds; 


3 


// Holds a latitude and longitude expressed as T_GEODETICs 
Semuct latLongPosition { 

TaGHOCBETIC latitude: 

T_GEODETIC longitude; 
i 


// Holds a grid position 
shevaihes cums aanlge lan 

double x,y; 2: 
hi 


of 


// 3 X% 3 mat was 
StEruUCe Mater 4 
float element [2 ii oie 


Lie 


// 3 X% 1 matrix or vVecror 
struct vector { 
float element[3]; 


yy 


// Oversize area to hold a GPS message 


typedef 


BYTE GPSdata[2 * GPSBLOCKSIZE] ; 


// Defines a type for holding compass messages 


typedef 


BYTE compDatal2 "  GoOMPsiZEl: 


// Structure for passing around various types of INS information. 
// The positions in the sample field of a stampedSample structure 


// sample[0]): x acceleration 
// sample[{1l]: y acceleration 
// sample[2]: z acceleration 
// sample[3]: phi 

// sample[4]: theta 

// sample[{5]: psi 

// sample[{6]: water speed 

// sample[7]: heading 


struct stampedSample { 


Grid est. 
float rawSample[8]; 
double sample[8]; 


//position as estimated by the INS. 
//Original readings for post processing 
//sampler converted sample. 


float deltaT:; 
float cuUrzent:. 


ee 
#endiff 


B. LOCATION.H 


//COnversion constants. for  locatien ot 


a ope 
#define 
#define 
#define 


#define 


#define 
#define 
#define 
#define 
#define 


#define 


-42.2Neandi 2 lis2 1 2o57 


LatToFt 0.10134 //converts degrees Latitude to ft 
LongToFt 0.08156 //converts degrees Longitude to ft 
HemisphereConversion -1 //-1 if west of of Greenwich 
RADIANMAGVAR 0.261799 // Local area Magnetic variation in radians 
xyAccelLimit ONE_G// Max accell in x and y diretion 

zAccelLimit 2 * ONE -G@ 7/7 Max accel in 2 directic7 

rateLimit 0.872665// Max rotational rate in radians 


speedLimit 25.3 //Max water speed 
headingbhimit 2°" ieee? 


psca levi) yro! 
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#define 
#define 


#define 
#define 
#define 
#define 
#define 


#define 


#define 


#define 


#define 


#define 
#define 


#define 


#define 
#define 


qeeale ah.0) //pitch 
rScale 1.0 //yaw 


pneecelocale 134° °/7/71.078 //pitch 
yAccelScale 3.895 //roll 


ZAGeelGeale 1.34 //1.038 

pUMmEES angular) (pScale ~ {((angular-2047.0) / 2047.0 ) 
rao Oe Mor T/ 1380.0) ) 

qUnits(angular) (qScale * (((angular-2047.0) / 2047.0 ) 
So UO) a ee MPT, 180.20) ) 

rUnits(angular) (rScale * (((angular-2047.0) / 2047.0 ) 
ese 0) emu. Preis 0 0) ) 


xAccelUnits(linear) (xAccelScale * ((linear-2047.0) / 


Z04)7 20°) * GRAVITY ) 
yAccelUnits(linear) (yAccelScale * ((linear-2047.0) / 
ZU4a) 0.3m GRAVELY ) 
zAccelUnits(linear) (zAccelScale * ((linear-2047.0) / 
uo Oe (Ol YS GRAV IFY ):) 


waterSpeedScale 1.0 //1.827 


depthUnits(depth) (((depth - 819.0) / (4095.0-819.0)) 


a BO") 
waterSpeedUnits (speed) (waterSpeedScale * ((speed - 
Z0AI=)) ( 20438 0) “ 25.3) //fLeet per second 
radToDeg(180.0/M_PI) 
degToRad (M_PI/180.0) 
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C. TOWFISH.CPP 


#include 
#include 
#include 
#include 
#include 


#include 
#include 


<Stadbib- ne 
<Sstato.n- 
St ime lo 
<liostream.h> 
FSIS MOA. BE. 


"toetypes.h" 
aye neoal ate 


int breakHandler (void); 
void screenSetUp (void); 
void printPosition (const latLongPosition&) ; 


[RE RRRRRRRRER KE K&R REE KK RRA a RE EE eee 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 


RETURNS : 


Main 

Eric Bachmann, Dave Gay 

teu stools 

Drives the navigator and its associated software. Counts 

the positions and displays each to the screen. Exited only when 
control break is entered at the keyboard. 

0 


CALLED BY:none 


CALLS. 


initializeNavigator (nav.h) 
navPosit (nav.h) 

DriNnt Position 

breakHandler 


KKKKKKEKKEKKKKE KK ERR ER EEK REA BRK HK AR ee ee eee 


Le yate 


main (int arge, char “argv/]) 


{ 


ctrlbrk(breakHandler); // trap all breaks to release com ports 
Setcpore hy: // turn break checking on at al eimes 


char *dataFile, *scriptFile, *attitudeFile; 


switch (arge) { 


case 2: 
scriptFile = new char[strlen(argv[1])]; 
strepy (scriptFile, argv([i]);//explicit seripemraleron 
dataFile = "data";//default raw data file 
aAttituderi ley] attr tude = 
break; 

case 3: 
scriptFile = new char[strlen(argv[1])]; 
strepy (scriptFile, argv[1));/7/explicit scripteeure 
dataFile = new char[strlen(argv[2])]; 
strcpy (dataFilepmarav(21-7 “explicit. data file 
attitudeFile =)Yateieude,; 


break; 
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case 4: 
scriptFile = new char{strlen(argv[1])]; 
SeLerylscrlpemle, argy{l!);//explicit script file 
dataFile = new char[strlen(argv[2])]; 
strcepy(dataFile, argv{2]);//explicit data file 
attitudeFile = new char[strlen(argv[3])]; 
Setaepyiatettuderale,,argvyi13)]);//explicit attitude file 
break; 


default: 
Seminch i lee— script". //detault script file 
dataFile = “rawdata";//default raw data file 
Geeatvcerile = “attitude”; 


elrscr {) ; 


Pome —- "\nWriting script information to " << scriptFile; 
@eue << "\nWriting binary data to “ << dataFile; 
Seute<< “\nWriting attitude data to " << attitudeFile << endl; 


Someme< "\ninitializing . . ."; 


//Instantiate the navigator 
navigator navl(scriptFile, dataFile, attitudeFile); 


latLongPosition currentLocation; // Lat/Long of most recent fix 
Boolean fixReceived = FALSE;//True if a new fix was recieved 
ine Pi<eouUMe=O- 0 7/ Count Of Navigation fixes recieved 


//Initialize the navigator 
currentLocation = navl.initializeNavigator (); 


Giatoxy (1,6); 
Seume-— “ta7tializaction Complete! \n"; 
Couto —— "Initial Position: \n": 


(7 eeint the initial position 
Souee.< “"Iatatude: “ << currentLocation.latitude.degrees << ':' 
—— CUrrentlLocation.latitude.minutes << ';' 
<< currentLocation.latitude.seconds << endl; 
cout << “longitude: " << currentLocation.longitude.degrees «<< ': 
<< currentLocation.longitude.minutes << ':' 
<< currentLocation.longitude.seconds; 


screenSetUp(); 


while (TRUE) { 
// Attempt to get a fix from the navigator 
fixReceived = navl.navPosit (currentLocation) 
fieetrdixheoecived) { 
// New fix recieved 
Gotkexy (Cc, 11) >; 
Coulee 44+L1xCount ; 


61 


printPosition(currentLocation) ; 


| eee ee ace ee a ES a ee EE EEE REE ERE ERK ERE 


PROGRAM : print Posi ten 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: Pius 1995 

FUNCTION: Displays position to the screen 
RETURNS: void 

CALLED BY: mail 

CALLS: none 


ae ee ee eee ar re es a KK EERE KEK REE REE KERR KKEEEKE REAL 


void 
printPosition (const latLongPosition& posit) 
{ 
gotoxy (11,14); 
cout << posit.latitude.degrees «<< ':' 
<< posit.latitude.minutes << ':' << posit.latitude.seconds << endl; 
detox (125015); 
cout << posit.longitude.degrees << ':' 
<< posit.longitude.minutes << ':' << posit.longitude.seconds << endl; 


ee eee ee en eK EKER ERK KE REE RAR REAR RAKE ERE RE ER ER ERM XR RES 


PROGRAM: breakHandler 


AUTHOR : Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Cleans up com ports upon program exit. 
RETURNS : 0 

CALLED BY: main 

CALLS: cleanup (portBank.h) 


KEKKEKEKKKEKKEKKEKKEKEKKEKKEKKEKEREKKKEKKKEKKKEKKKKKKKKKKKKKKEKKKKEKEKEKEKEEKEKKKEKE | 


ari 
breakHandler (void) 
{ 
COMports.cleanup(); 
exit. (07; 
return 0; // keep the compiler happy 
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RII IIR III II III I III III I II II IIR RI te ee eke ee ae 


PROGRAM: screenSetup 

AUTHOR:Eric Bachmann, Randy Walker 
DATE:12 May 1996 

FUNCTION:Sets up the output screen 


RETURNS : 0 

CALLED BY: main 

CALLS : none 

etter es KEE EEE RRA K RRR AEE ERR EEE ER AER KE ERR RK ARR 
void 

screenSetUp (void) 

{ 


goemoxy (4,11); 
Comte << "Fix "> 


gotoxy (1,14); 
Gemeewa<e “Latitude: " << "\nLongitude: "; 


gocoxy (1,17); 
come <a" Roll: “* << @#A\nPitch: "; 


mecoxy (1,25); 
cout << “deltaT: "; 


ie COol (45) ,row(1)>; 


gotoxy (col, row++) ; 
Geum, << "x accel: "; 
gotoxy (col, row++) ; 
meme << “"y accel: " 
gotoxy (col, row++); 
Geut <<e"Z acce bi" 
gotoxy (col, row++); 

Geoue << “phi dot: "; 
gotoxy (col, row++) ; 

Beme << “theta dot: "; 
gotoxy (col, row++); 
Coule=< “psiedot« "; 
gotoxy (col, row++) ; 

cout << “water speed: "; 
gotoxy (col, row++); 

Gouc << “heading: "; 


=e 


col AS 
mow = 12> 


gotoxy (col, row++); 
eat << “x: "> 
gotoxy (col, row++); 
some << “ys "> 
gotoxy (col, row++); 
eat << "ZF: "> 
gotoxy (col, row++) ; 
COut gas Pini "> 
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gotoxy (col, rowrae 
cout << “theta: wae 
gotoxy (col, row++) ; 
CCl, =—= “Psi: sae 


gGotoxy (45,20) ; 
cout << "Bias Values"; 


Goroxy (60720); 
cout << "Current Values"; 


D. NAV.H 


#ifndef —_ NAVIGATOR_H 
#define | NAVIGATOR_H 
#include <stdio.h> 
#include <fstream.h> 
#include <iostream.h> 
#include <math.h> 


#include "toetypes.h" 
#include  “Gqps aa. 
Finclude "ans se 
#include “location.h" 


// Converts milseconds to degrees 
#define MSECS TO DEGREES (15°07 (1000.0 * 3600.0) ) 


[RRR ERKERKEREKEREEREKERRERER EEK ARERR KEKE EEKE RRR E RR EK RS AS ee ee ee 


CLASS: navigator 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: Ai 1 OS 


FUNCTION: Combines GPS and INS information to return the eur rene 


estimated position. 
RRR ERRR EEE EE KAR KH Re RK KE RK I Ree 


class navigator { 
pupiiie: 


//Constructor, opens script and data files 
navigator(char scriptFile[] = “navScript",char dataFile[] = "navDaeaum 
char attitudeFile[] = "navatt"): positionData (scripenile) 


attitudeData(attitudeFile),elapsedTime(0.0), fixCount(0), gpsSpeedSum(0.0), 
insSpeedSum(0.0) 


{if ((rawData = fopen (datakile,” Ywee) so NULL es 
cout << "NO RAW DATA RECORD"; ) 


//Destructor, closes script and data files 
~navigator() {positionData.close();attitudeData.close();fclose(rawData) ;} 
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//provides the navigator's best estimate of current position 
Boolean navPosit (latLongPosition&); 


//Initialize the navigator 
latLongPosition initializeNavigator(); 


private: 


float elapsedTime; // Tracks time since navigator was initialized 
tine £1xCount ; // the number of position fixes obtained 


double gpsSpeed, insSpeed; 
double gpsSpeedSum, insSpeedSum; 


INS insi;//INS object instance. 
GPS gpsi;//GPS object instance. 


ofstream attitudeData;// Post processing attitude data. 
ofstream positionData; // Position script file 
FILE *rawData;// Post processing binary data file. 


TathongMilSec origin; //lat-long of navigational origin 


//Write position information to script file 
void writeScriptPosit(int, latLongMilSec&, char); 


//Write an INS packet and its timeStamp to the outPut file 
void writeInsData(const stampedSample& drPosition); 


//Write a GPS message to the outPut file. 
void writeGpsData(const GPSdata& satPosition) ; 


//Returns the position in Miliseconds 
latLongMilSec getMilSec(const GPSdata&) ; 


//Convert position in milSec to degress, minutes, seconds and milsec 
latLongPosition milSecToLatLong(const latLongMilSec&) ; 


//Convert xy (grid) position to lat long 
latLongMilSec gridToMilSec(const grid&); 


Vreonverts Vat/long to xy position 
grid milSecToGrid(const latLongMilSec&) ; 


//Parses and returns the time of a GPS message. 
double getGpsTime(const GPSdata& rawMessage) ; 


//Parses and returns the velocity in fps of a GPS message. 
double getGpsVelocity(const GPSdata& rawMessage) ; 

ia 

#endif 
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E. NAV.CPP 


#include "nav.h" 
#include "signal .h" 
#define SIGFPE 8// Floating point exception 


[RRR RR RREERERE REE EAE SR KS ER eee 


PROGRAM: navPosit 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: Pivsuly = 1 oo 
FUNCTION: Provides the navigator's best estimate of current position. 
Attempts to obtain GPS and INS position fixes from the gps 
and ins objects and copies the most accurate fix available 
into the input argument ‘navPosition'. Writes the raw position 
fix data to the output file for post processing. Sets a return 
flag to indicate whether a valid fix was obtained. 
RETURNS: TRUE, a valid position fix is in the variable 'navPosition'. 
FALSE, otherwise. 
CALLED BY:towfish.cpp (main) 
CALLS: gpsPosition (gps.h) 
GOrrect PoOsiGicon (imesh 
TnSPositaon (ans i 
getMilSec (nav.h) 
gridToMilSec (nav.h) 
milSecToGrid (nav.h) 
milSecToLatLong (nav.h) 
writeScriptPosit (nav.h) 


KKK ERE HK KE EK EK EERE Ke EEK he RRR EEK KS KK EE RE ER Ke RR Ae ee 


void fpeNavPosit (int sig) 


{if (sig == SIGFPE) %cerr << “floating point ecrver im Navro-1 mae 
Boolean 

Navigator::navPosit (latLongPosition& navPosition) 

{ 


signal (SIGFPE, fpeNavPosit) ; 

GPSdata satPosition; // the latest GPS position 

stampedSample drPosition; // the latest INS position 

latLongMilSec gpsMilSec; // the latest GPS position in milseconds 
latLongMilSec insMilSec; // the latest INS position in milseconds 


//Attempt to get the INS and GPS positions 
Boolean insFlag = insl.insPosition(drPosition) ; 
Boolean gpsFlag = gpsl.gpsPosition(satPosition) ; 


//INS and GPS positions obtained? 
if (insFlag && gpsFlag) { 
dotenmy. (20, iis 
cout << “GPS. 
// Write INS packet and attitude info to an output file 
elapsedTime += drPosition.deltaT; 
writeInsData (drPosition) ; 
//Write GPS message to output file. 
writeGpsData(satPosition) ; 
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//Parse position from GPS messsage 
gpsMilSec = getMilSec(satPosition) ; 
//Write milsec position to script file 
writeScriptPosit (++f1ixCount, gpsMilSec, 'G'); 
//Pass GPS position to INS object for navigation corrections. 
insl.correct Position (milSecToGrid(gpsMilSec), getGpsTime(satPosition) ); 
//Covert position in mil sec to latitude and longitude. 
navPosition = milSecToLatLong(gpsMilSec) ; 
Peturn TRUE; 
} 
else { 
//Only INS position obtained? 
mt (insklag) { 
Goro<y (20711); 
COU << ~ wee 
// Write INS Packet to output file. 
elapsedTime += drPosition.deltaT; 
writeInsData(drPosition); 
insMilSec = gridToMilSec(drPosition.est); 
//Weite milsec position to script file 
writeScriptPosit (++f£1xCount, insMilSec, '‘'I'); 
navPosition = milSecToLatLong(insMilSec) ; 


insSpeed = drPosition.sample[6]; 


return TRUE; 
} 
else { 
// Only GPS position obtained? 
ff ({qpsrlaq) { 
Goce (20) 11); 
COut << "GPS": 
// Write GPS message to output file. 
writeGpsData(satPosition) ; 
//Parse position from GPS messsage 
gpsMilSec = getMilSec(satPosition) ; 
//Write milsec position to script file 
writeScriptPosit (++f1xCount, gpsMilSec, '‘'G'); 
//Pass GPS position to INS object for navigation corrections. 
insl.correct Position (milSecToGrid(gpsMilSec), 
getGpsTime(satPosition)); 
//Convert position in mil sec to lat/long. 
navPosition = milSecToLatLong(getMilSec(satPosition)); 


gpsSpeed = getGpsVelocity(satPosition) ; 
gpsSpeedSum += gpsSpeed; 

insSpeedSum += insSpeed; 

Getonm, too, 7): 

cout << gpsSpeed; 

Goro (50, b0) ; 

cout << gpsSpeedSum / insSpeedSum; 


Bervrne ll nUE: 
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else { 
return FALSE; // No new position available 


} 


[RRR KERR RRR REA RES A ee ee ee eee 


PROGRAM : writeScriptPosit 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Writes the fix number, the position in milSec and the type 
of fix Eo the wseripe tf. )e- 

RETURNS: void 


CALLED BY: navPosit (nav.cpp) 
initialPosit (nav.cpp) 
CALLS: None 


KEKKKKEKEKKKKKKKEKKEKEKEKKEKEKEKEKREEKKRKERKEKKEKEKEKRERKKKEKK KKK KEKEKKK KKK EK KKK ee 


void 
navigator: :writeScriptPosit (int fixNumber, latLongMilSec& posit, char f1ixType) 
{ 
positionData << fixNumber << ' ' 
<< posit.latitude << ' ' 
<< DOsit wlenGgitude <<a 
ac. hixl ype <0. 
<< elapsedTime << endl; 


[EERE EER ER EERE RE RRR EE EK RK RK AK RA RR ee 


PROGRAM: writeInsData 


AUTHOR: Eric Bachmann, Dave Gay 

DATE: Pil eeanwl es AVS) Shs: 

FUNCTION: Writes the packet and the time stamp contained in a stamped 
sample to the out put file for post processing. 

RE LURNS: void 

CALLED BY: navPosit (nav.cpp) 

CALLS: None 


KAEKKKEKEKKERKKKEKKKRKKKEEKEKEEKEEREAEK KERR ERRE RK 0 ee eee 


void 
navigator::writeInsData(const stampedSample& drPosition) 


{ 
//MUST ADD CODE TO RECORD DATA FOR POST PROCESSING HERE 


//Output attitude data to a file 

attitudeData << elapsedTime << ' ' 
<< drPosition.sample [0] '2.5 
<< -1.0 **drPosition cample Ll). 
<< drPosition.sample[2] << ' ' 
<< (radToDeg * drPosition cample(3|]}y-. = 
<<, (radTobeg)* =drPosition-sample|[4) ji... 
<< (radiobeq»* dxrPosition.samplels)))) 2... 
<< drPosition.sample[6] << ' ' 
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<< (radToDeg * drPosition.sample[{7]) << ' '! 
=—wearPositton.current << endl; 


} 


a a EEA ERE KKRREKERERERKERKKEEKKEEKKEKREKEKEKEKEEKKKE 


PROGRAM : writeGpsData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Writes a raw GPS message to a binary output file for 
post processing. 

RETURNS : void 

CALLED BY: navPosit (nav.cpp) 

CALLS: None 


MEER ital. een Gane a A RARE RAR KK REAR AKKKKRE KERR AKEKKKKAKKK KKK / 


Mord 
navigator: :writeGpsData(const GPSdata& satPosition) 
{ 
imine | = O- 7 < GPSBLOCKSIZE-  3++) { 
putc(satPosition[{j], rawData); 


} 

a ERR EK KEKARRXKKARKKKEKKAEKKAKKKEK 
PROGRAM : initializeNavigator 

AUTHOR: Eric Bachmann, Dave Gay 

DALE: ae vieles 1b) o.S 

FUNCTION: Obtains an initial GPS fix for use as a navigational origin for 


grid positions used by the INS object. Saves the origin and passes 
it to the INS object in latLong form. 


PeLuURNS: TRUE 

CARLED BY: towfish (main) 

CALLS: GpsPosition (gps.cpp) 
Correct Position (ins .cpp) 
getMilSec (nav.cpp) 
milSecToGrid (nav.cpp) 


ee ee A Re A ERR EAE KERR KKK EKER REE KEKEER KEKE / 


latLongPosition 
navigator: :initializeNavigator () 
{ 


stampedSample biasPackets {biasNumber]; 
GPSdata satPosition; //gps position message 


// Loop until an initial GPS fix is obtained. 
while(!gpsl.gpsPosition(satPosition)) { /* */ } 


// Write GPS message for the grid origin to output file. 
writeGpsData(satPosition) ; 

//Save navigational origin for later grid position conversions. 
origin = getMilSec(satPosition) ; 

//Write the initial position to the script file 
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writeScript Posit (07 @erdqume ew 
//Pass time of first GPS fix to INS object initialization routine. 
insl.insSetUp (getGpsTime(satPosition), biasPackets) ; 


for (int i = 0; i = BbiasNumber-) 442) 
writeInsData(biasPackets[i]); 


} 


insSpeed = biasPackets[biasNumber - 1].sample[6]; 
gpsSpeed = getGpsVelocity(satPosition) ; 


//Return the initial pescteven Gauche caller. 
return milSecToLatLong (origina 


[RRR ERER EERE EE RRREREEAEAREH RARE AAS eee 


PROGRAM: getMilSec 
AUTHOR:Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Extracts a position in mili seconds from a Motorola (@@Ba) 
position contained in the input argument 'rawMessage' and returns it. 
RETURNS:The latitude and longitude in milseconds. 
CALLED BY: navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS:none. 


KEK KKK RR RA KH EAR AR IR RR ee ee ee eee ee 


latLongMilSec 
navigator: :getMilSec(const GPSdata& rawMessage) { 


FOURBYTE temps4byte; 
latLongMilSec position; 


temps4byte = rawMessage[15]; 

temps4byte = (temps4byte<<8) + rawMessage[16]; 
temps4byte = (temps4byte<<8) + rawMessage[17]; 
temps4byte = (temps4byte<<8) + rawMessage[18]; 


position.latitude = temps4byte; 


temps4byte = rawMessage[19]; 

temps4byte = (temps4byte<<8) + rawMessage[20]/; 
temps4byte = (temps4byte<<8) + rawMessage[21]; 
temps4byte = (temps4byte<<8) + rawMessage[22]; 


position.longitude = temps4byte; 


return position; 
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REAR ARA ERE KKERKEKEKKERKEKEEREE RAKE 


PROGRAM: milSecToLat Long 

AUTHOR:Eric Bachmann, Dave Gay 

ieee ll! July 1995 

FUNCTION:Converts a position expressed in totally in mili seconds to 
degress, minutes, seconds and mili seconds and returns the result. 

RETURNS: The position in degress, minutes, seconds and mili seconds. 

CALLED BY: navPosit (nav.cpp) 

CALLS :none 


inne nee Dee he ee ARERR KEK ER ARAKRAERERERERKEKKKKKKKEKKREKEE | 


latLongPosition 
navigator: :milSecToLatLong(const latLongMilSec& milSec) { 


latLongPosition position; 

double degrees, minutes; 

degrees = (double)milSec.latitude * MSECS_TO_DEGREES; 
position.latitude.degrees = (TWOBYTE) degrees; 


1f (degrees < 0) 
degrees = fabs (degrees) ; 


Minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.latitude.minutes = (TWOBYTE)minutes; 


position.latitude.seconds = (minutes - (TWOBYTE)minutes) * 60.0; 


degrees = (double)milSec.longitude * MSECS_TO_DEGREES; 
position.longitude.degrees = (TWOBYTE) degrees; 


i1f(degrees < 0) 
degrees = fabs(degrees); 


minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.longitude.minutes = (TWOBYTE) minutes; 


position.longitude.seconds = (minutes - (TWOBYTE)minutes) * 60.0; 


return position; 
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[RRR RRR RK KR KR RR KR RR ee 


PROGRAM: gridToMilSec 

AUTHOR:Eric Bachmann, Dave Gay 

DATE: 11 July ie os 

FUNCTION:Convert a grid position to a latitude and longitude in mil- 
seconds and returns the result. 

RETURNS:The latitude and longitude in milseconds. 

CALLED BY:navPosit (nav.cpp) 

CALLS :none 

HR KKK ee RR He eR RR KR ee 

void fpeGridToMilSec(int sig) 

{if (sig == SIGFPE) cerr << “floating point error in qridtoMi see mas 


latLongMilSec 
navigator: :gridToMilSec(const grid& posit) 
{ 
Signal(SIGFPE, fpeGridToMilSec) ; 
latLongMilSec latLong; 


//converts grid in ft to latitude 

latLong.latitude = origin.latitude + (posit.x / LatToFt); 

/(/Converts Grid in Eeweomlongitude 

latLong.longitude = origin. longitude = 
HemisphereConversion * (posit.y / LongToFt) ; 

return latLong; 


[RRR KRRKRKEEKREKRKEEER KE ERR ER ERR RK eee 


PROGRAM :milSecToGrid 
AUTHOR:Eric Bachmann, Dave Gay 
DATE: Li July 1995 
FUNCTION:Convert a latitude and longitude expressed in milseconds to 
a grid position based on the lat/long of the grid origin. 
RETURNS:The grid position 
CALLED BY:navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS :none 
COMMENTS :altitude is always assumed to be zero. 


KERR EK EEK ERE RRR RK RK ER RR eee ee ee 


//Converts latitude/longitude to xy coords Gin vte from origin 
‘epee 
navigator::milSecToGrid(const latLongMilSec& posit) 


{ 


Grid “positions 


position.x = (posit.latitude - origin. latitudes -suaelo.e, 
position.y = HemisphereConversion * 

(posit .longitude — origin. longitude) =e beng tek, 
Posie lon. 2 = 0 


return positron, 
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ng Re ee EAR EAR KERR EE KA RK KKKKEKEKEEREKRK KKK 


PROGRAM: getGpsTime 
AUTHOR:Eric Bachmann, Dave Gay 
Petes tt July 1995 
FUNCTION: Parse the time of a gps message. 
RETURNS:The time of the gps message in seconds 
CALLED BY:navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 

CALLS : none 
Se eae ee a en EERE A EERE RE EEE ERE RE REREEEKREEKRR KKK / 
double 
navigator::getGpsTime(const GPSdata& rawMessage) 
{ 

UNSIGNED_ONEBYTE tempchar, hours, minutes; 

UNSIGNED_FOURBYTE tempu4byte; 

double seconds; 


hours = rawMessage [8]; 

minutes = rawMessage[9]; 

tempchar = rawMessage[10]; 

tempu4byte = rawMessage[11]; 

tempu4byte = (tempu4byte<<8) + rawMessage[12]; 
tempu4byte = (tempu4byte<<8) + rawMessage[13]; 
tempu4byte = (tempu4byte<<8) + rawMessage[14]; 
seconds = (double)tempchar + (((double)tempu4byte)/1.0E+9) ; 


fecucm hours * 3600.0 + minutes * 60.0 + seconds; 


Reese CORA Re oe en ee ee Kr Ke ee AERA RAKE 


PROGRAM: getGpsVelocity 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION:Parse the velocity out of a gps message. 
RETURNS:The velocitiy of the gps message in feet per second 
CALLED BY:navPosit (nav.cpp) 

initializeNavigator (nav.cpp) 


CALLS : none 


KKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK / 


double 
navigator: :getGpsVelocity(const GPSdata& rawMessage) 
{ 

UNSIGNED_ONEBYTE tempchar=rawMessage [31]; 


return (double) (3.2804 * ((tempchar << 8) + rawMessage[32]) / 100.00); 


i 


F. GPS.H 


#ifndef _GPS_H 
#define _GPS_H 


finGelude <lostreamen— 
#include <fstream.h> 
#inebuide =—conpro.h> 


#include “portbank.h" 
#include "toetypes.h" 
#include "gpsbuff.h" 


[RRR EERE KERR K REE KREER KEE EK RAK AKA Kh ee 


CLASS: gps 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 1 dul slg os 

MODIFIED: 15 May 1996 by Eric Bachmann & Randy Walker 

FUNCTION:Reads GPS messages from the GPS buffer. Checks for valid 
checksum and minimun number of satelites in view. 


FOI II I I I I II IO II II III III I I I II II II II IO III IO ORR I Ik FO IK / 
class GPS { 
pub ric: 
// Class VCOnskEruceo: 
GPS() = portI{GOMports tne (CoM, SYTE(4) 2 sbecuur 


NOPARITY, BYTE(8), BYTE(1), XON_XOFF, messages)) { } 


//returns the latest gps position anda flag 
Boolean gpsPosition (GPSdata&) ; 


private: 


//puffer for gps data 
GPSbuffer messages; 


//instantiates serial port communications on comm2 
bufferedSerialPort& portl1; 


//calculates the check sum of the message 
Boolean checkSumCheck (const GPSdata); 


a 
#endif 
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G. GPS.CPP 


Pome lude <math.h> 
fanmelude “qps.h" 


eR ae RR EK RERKEREKEKERAEREKKKKEKREKREKEKEKEREEKKE 


NAME: gps Position 
AUTHOR: Eric Bachmann, Dave Gay 
DATE:11 July 1995 
MODIFIED: 15 May 1995 BY Eric Bachmann & Randy Walker 
FUNCTION:Determines if an updated gps position message is available and 
copies it into the input argument ‘rawMessage'. If the message 
has a valid checksum, was obtained with at least three 
satellites in view, and contained the differential correction, a ‘TRUE' 
is returned to the caller,indicating that the message is valid. 
RETURNS: TRUE, if a valid position message is contained in the 
input argument. 
CALLED BY: navPosit (navigator .h) 
@emeco-cet (bufifer-h) 
checkSumCheck (gps.h) 


eee KK KAR KARE KEE KAKA ERK ERE KERR KE RERE KEKE KKKKKERKEEKEKKKKKEKE / 


Boolean 
GPS::gpsPosition (GPSdata& rawMessage) 
{ 


Boolean checkSumFlag; 
unsigned long Mask(4); 


if (messages .Get(rawMessage)) { 
// Check for a valid check sum and more the 3 satelites and DGPS 


return Boolean((checkSumCheck(rawMessage)) && (rawMessage[39] > 3) 
&& ((rawMessage [GPSBLOCKSIZE - 4] & Mask) == Mask)); 


} 


else { 
return FALSE; // No updated position is available. 


is 


[RRR E KERR KARE RRR R EK EER ee 


PROGRAM: checkSumCheck 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July Loss 

MODIFIED: 15 May, 1996 by Eric Bachmann & Randy Walker 

FUNCTION: Takes an exclusive or of bytes 2 through 78 in a Motorola format 
(@@EA) position message and compares it to the checksum of the 


message. 

RETURNS: TRUE, if the message contains a valid checksum 
CALLED BY: .gpsPosition (gps) 

CALLS: none 


HH eH a KK EK KK RR KR RK KR KK eee ee 0 ee 


Boolean 
GPS: :checkSumCheck (const GPSdata newMessage) 


{ 
BYTE chkSum(0) ; 


for (1nt 1-= Zo a = CrSEbOGh sie es ae et 
chkSum *= newMessage[i]; 


} 
return Boolean(chkSum == newMessage[GPSBLOCKSIZE - 3]); 


H. INS.H 


#ifndef _INS_H 
#define _INS_H 


#include 
#include 
#include 


=Eime.n- 
<math.h> 
<dos.h> 


#include <stdio.h> 
#ineclude <eon1ic6. h- 
#include <fstream.h> 
#include <iostream.h> 


#include "toetypes.h" 
finclude “location. kh’ 
#include "sampler.h" 


[RRR RRR RR RRR RRR I ee ee 


CLASS:ins 

AUTHOR:Eric Bachmann, Dave Gay 

DATE: 11) duty 1995 

FUNCTION: Takes in linear accelerations, angular rates, speed and 
heading information and uses kalman filtering techniques to return 


a dead reconing position. 
Kee ke KK Ke RI Ke ee KK RR RRR RK KR KR RR KK AK EA ee EE KR ee 


class INS { 


Dulodiare 
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V7 CONSeructor initializes gains 
INS (); 


SANS () (} 


//ceturns the ins estimated position 
Boolean insPosition(stampedSample&) ; 


//Updates the x, y and z of the vehicle posture 
Vold "GorrectEPosition(const grid&, double); 


//Sets posture to the origin and developes initial biases 
void insSetUp(double, stampedSample?*) ; 


private: 

double posture[6]; // ins estimated posture (x y z phi theta psi) 

double velocities[6]; // ins estimated linear and angular velocities 
// x-dot y-dot z-dot phi-dot theta-dot psi-dot 

double current [2]; // Sinseestimated Gerrer current 
// (x-dot y-dot z-dot) 

double lastGPStime; //time of last gps position fix 

sampler saml; //sampler instance 

matrix rotationMatrix; //bpody to euler transformation matrix 


double biasCorrection[8];//Software bias corrections for IMU rate 
//sensors 


// Kalman filter gains. 
float Konel, Kone2, Ktwo, Kthreel, Kthree2, Kfourl, Kfour2; 


// Finds the difference between two times of struct time type 
double findDeltaT (struct time& next, struct time& last); 


// Transforms from body coordinates to earth coordinates 
// and removes the gravity component 


Vold transformAccels (double[]); 


// Transforms water speed reading to x and y components 
void transformWaterSpeed (double, double[]); 


// Tranforms body euler rates to earth euler rates. 
void transformBodyRates (double[]); 


// Euler integrates the accelerations and updates the velocities 
void updateVelocities (stampedSample&) ; 


// Euler integrates the velocities and update the posture 
void updatePosture (stampedSample&) ; 


qa 


// Builds the body to euler rate matrix 
matrix buildBodyRateMatrix(); 


// Builds the body to earth rotation matrix 
void bud ldRGtat tenMarGisa), 


//Calculates the imu bias correction during set up 
void calculateBiasCorrections (stampedSample*) ; 


//Applies bias corrections to a sample 
void applyBiasCorrections (double sample[]); 


es 


// Post multiply a matrix times a vector and return result. 
vector operator* (matrix&, double[])); 


#endif 


I. INS.CPP 


#include <iostream.h> 

#include "signal -.h" 

#include "ins.h" 

#define SIGFPE 8// Floating point exception 


[BR RRR RRR RRR RK RRR RR RR RII TR ER ee 


PROGRAM: ins (constructor) 

AUTHOR:Eric Bachmann, Dave Gay 

DD AREeS bees lee ois 

FUNCTION:Constructor initializes kalman filter gains and linear and 
angular velocities. 

RETURNS: nothing 

GALLED UBY : navigator class 

CALLS: none 


KKEKKEKREK RA RR EKA Ke ERK RK KR EK RRS RR ee 


INS *- INS () = Konel(Ge ll) Kene2 (07s )- 
Kewot 0. 6). 
Kthreel1(0.5), Kthree2(0.5), 
Kfourl(075)2. kh fours (0s) 


velocities (0) = 9020.7 / waco 
velocities[1] = 0.0-/7 y dot 
velocities (2Z],-= 0.077 e226 


velocities(3]) = 
velocities[4] = 
velocities[5] = 


20c// Dit weet 
70>) /mtneta cdot 
AUEy a) jor iclohs 


QO QO © Oo © 


//Set posture to straight and level at the origin. 
posture (0) = 9 0e0- 
peste (li =a0 Ue 
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posture[2] 
posture [3] 
posture[4] 
posture[5] 


| | 
8 IES Eb ED PO wa 


i HRT wo a8 ay ST ae 


™e 


fy initialize error current to zero 
current [0] 
eurrent [1 ) 
current [2] 


I 
Oo © © 
Co 


=e 
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PROGRAM: findDeltaT 

AUTHOR:Eric Bachmann, Dave Gay . 

memes ll July 1995 

FUNCTION:Converts two times stored in the time structure type of 
dos.h into the time in seconds and returns the difference. 

RETURNS: difference in seconds between the two input times. 

CALLED BY: insPosit (ins.cpp) 

CALLS: none 

oe TAKE AK KKK KKK KKK ARKAKEKKAEKKAKKEKKEKEEKEKKKEKRKEKEKEKEKKEERKEKKKKEEKEKKEKKEKK / 

double 

INS::findDeltaT (struct time& next, struct time& last) 

{ 


double present, past; 


pHeseme = next .ti_hour * 3600.0 + next.ti_min * 60.0 
+ next.ti_sec + next.ti_hund / 100.0; 
Eom last.ti hour *~ 3600.0 + last.ti_min * 60.0 
+ last.ti_sec + last.ti_hund / 100.0; 


// Did 2400 occur between present and past? 
if (present < past) { 

present += 86400.0; 
} 


return present - past; 


li 
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PROGRAM: insPosit 
AUTHOR: Eric Bachmann, 
DATE:11 July 1995 


Dave Gay 


FUNCTION: 
filtering. Inputs are linear accelerations, 
heading. Primary input data is obtained 


getSample method. This data is stored in 


stampedSample structure called newSample. 


used as a working variable as the linear 


Make dead reckoning position estimation using kalman 


angular rates, speed and 
from a sampler object via the 
the sample filed of a 

The sample field is then 
accelerations and angular 


rates it contains are converted to earth coordinates and integrated 
to determine current velocities and posture. The data is complimentary 
filtered against itself, speed and magnetic heading. 

RETURNS: position in grid coordinates as estimated by the INS 


CALLED BY: navPosit (nav.cpp) 
CALLS: getSample (sampler.cpp) 
findDeltaT (ins.cpp) 


transformBodyRates (ins.cpp) 

buildRotationMatrix (ins.cpp) 

transformAccels (ins) 

transformWaterSpeed (ins) 
Hea KK KR RR A RRR RR RRR AR RIK RR RR ee 
void fpeInsPosit(int sig) 
{ake Teste SIGFPE) cerr << “floating point error Gy insrosie 1 


Boolean 
INS: :insPosition(stampedSample& newSample) 
{ 
Signal (SIGFPE, fpeInsPosit); 
double thetaA, phiA, xIncline, yIncline; 


double waterSpeedCorrection[3]; 


// Working variables 
// Filter correction #emed mie. 
// and water speed 

if (saml.getSample(newSample)) { 
applyBiasCorrections (newSample.sample) ; 


//Set output precision and fixed format 
CcOulL. precision 6) 7, 
COuE.SeEET (10S. fixed) 


//Display linear accelrations and angular rates 
for (ink 7} = 0s Sy eS ee 

Goroxy (594710): 

cout << newSample.sample[j]; 


//Display time delta to the screen. 
MGEOny (9-25 )5; 
cout << newSample.deltaT; 


tine lime 
Vine Line = 


newSample.sample[0] / GRAVITY; 
newSample.sample[1] / (GRAVITY * cos(posture[4])); 
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if (fabs(yIncline) > 1.0) { 
Statue  Intasme lineCoune (0); 
Gotexmy (1,24); 
Cee ine linationserrens: O" << ++inclineCount << endl: 
Feturn FALSE; 


//Calculate low freg pitch and roll 
thetaA = asin(xIncline) ; 
phiA = -asin(yIncline); 


//Transform body rates to euler rates. 
transformBodyRates (newSample.sample) ; 


//Calculate estimated roll rate (phi-dot). 


velocities[(3] = newSample.sample[3] + Konel * (phiA - posture[3]); 
//Calculate estimated pitch rate (theta-dot). 

velocities[{4] = newSample.sample[(4] + Kone2 * (thetaA - posture[4]); 
//Calculate estimated heading rate (psi-dot). 

velocities[(5] = newSample.sample[5] + Ktwo * (newSample.sample[7] - 


posture[5]); 


//integrate estimated pitch rate to obtain pitch angle 
posture[3] += newSample.deltaT * velocities([3]; 
//integrate estimated roll rate to obtain roll angle 
posture[4] += newSample.deltaT * velocities([4]; 
//integrate estimated yaw rate to obtain heading 
posture[5] += newSample.deltaT * velocities[5]; 


fy Pasplay roll and pitch 

Gorory (S,17); 

Gout << (posture[3] * radToDeg) ; 
geeoxy (oc, 18); 

cout << (posture[4] * radToDeg); 


buildRotationMatrix(); 


//Transform accels to earth coordinates 
transformAccels (newSample.sample) ; 


//Transform water speed to earth coordinates 
transformWaterSpeed (newSample.sample[6], waterSpeedCorrection) ; 


// Subtract out previous velocity and apply statistical gain 


waterSpeedCorrection[0] = Kthreel * (waterSpeedCorrection[0] - 
velocities[0]); 
waterSpeedCorrection[1] = Kthree2 * (waterSpeedCorrection[1] - 


velocities([(1]); 
// Determine filtered accelerations 


newSample.sample[0] += waterSpeedCorrection([0]; 
newSample.sample[1] += waterSpeedCorrection([1]; 
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//Integrate accelerations to obtain velocities 

velocities[0] += newSample.sample[0] * newSample.deltaT; 
velocities[1] += newSample.sample[1] * newSample.deltaT; 
velocities[2] += newSample.sample[2] * newSample.deltaT; 


//Integrate velocities to obtain posture 

posture[0] += (velocities[0] + current[0]) * newSample.deltaT; 
posture[1] += (velocities[1] + current[1]) * newSample.deltaT; 
posture[2] += velocities[2] * newSample.deltaT; 


newSample.current = sqrt(current[0] * current[0] + 
Currenct( 1) =“#egrreret |)- 


newSample.sample[0] = posture[0]; 
newSample.sample[1] = posture[1]; 
newSample.sample[2] = posture[2]; 
newSample.sample[3] = posture[3]; 
newSample.sample[4] = posture[4]; 
newSample.sample[5] = posture[5]j; 


//Display current location and posture 
for (9 = 03 ao =. 6G ee ed 

Gocoxy (327,44 2) 

cout << posture[j]; 
} 
newSample.est.x = posture[0]; 
newSample.est.y posture[1l1]; 
newSample.est.zZ posture (Zi; 


rFecurn | lRUG- 
} 
else { 
return FALSE;// New IMU information was unavailable. 


} 


[REE KEKE KR RARE REE ERK RR DR RR OR ee 


PROGRAM: correct Position 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Reinitializes the INS based on a known position and compute 
apparent current based on past accumulated errors of the INS. It is 
called by the navigator each time a new GPS (true) fix is obtained. 

RETURNS: void 

CALLED BY: navPosit (nav) 

CALLS : none 


Fe He He Te RI HN RK I I Re PIE I I ER Te AA A IO ee ee ee 


void 
INS: :correctPosition(const grid& truePosit, double positTime) 


{ 
double deltaT; 


// Correct for new day if necessary 
1f£ (positTime < lastGPStime) { 
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positTime += 86400; 


fer ind time since last gps fix. 
deltaT = positTime - lastGPStime; 


// Detemine INS error since last gps fix 
double deltaX = truePosit.x - posture[0]; 
double deltaY = truePosit.y - posture[1]; 


// Reinitialize posture to known position (gps fix) 
Peseure([0] = truePosit.x; 

posture[1] truePosit.y; 

posture [2] 0.0; //Unit is assumed to be on the surface 


I 


// Add gain filtered error to previous errors 
Gumrent|0)] += Kfourl * (deltaX / deltaT):; 
Smuerencec(|l] += Kfour2 * (deltaY / deltaT); 


// Display new error current values 
pomrint 7 = 0; j < 3; 74++) { 
Goeoxy (560, 3421); 
Gout << current [7]: 


// Display updated posture 
meee = 0; 3 < 6G; j++) £ 
qeboxm (52, +12); 
Gout e<< posture[j]; 


// Save the time of the gps fix for next calculation 
lastGPStime = positTime; 
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[RRR RERRRERKKRR KR R ORR he a ee 
PROGRAM: insSetUp 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: li gutyeoeoS> 
FUNCTION: Initializes the INS system. Sets the posture to the origin. 
Initializes the heading using magnetic compass information. Initilizes 
the times of the last GPS fix and last IMU information. 
RETURNS: void 
CALLED BY:initializeNavigator (nav) 
CALLS: calulateBliasCorrections (ins) 
getSample (sampler) 
buildRotationMatrix (ins) 
transformWaterSpeed (ins) 


KEKKKEKEEK KEK KERR E KEK KK RK RR ee 
void fpeInsSetUp(int sig) 
{if (sig == SIGFPE) cerr << “fEloeating point error im inser dp am | 


void 
INS: :insSetUp(double originTime, stampedSample* biasPackets) 
‘ 

Signal (SIGFPE, fpeInsSetUp) ; 


//Initialize the sampler 
saml.initSampler (); 


//set imu biases 
calculateBlasCorrections (biasPackets); 


//set initial true heading 
posture[5] = biasPackets[biasNumber-1]-.sample[7]; 


//set initial speed 
buildRotationMatrix(); 
transformWaterSpeed (biasPackets[biasNumber-1].sample[6], velocities) ; 


// initialize times 
lastGPStime = originTime; 


//Display initial error current values 
for(int ji 7s O==jy soo 14 tie 

gotoxy (60,j3+21); 

GOUL << Current |4\- 
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PROGRAM: transformAccels 
AUTHOR:Eric Bachmann, Dave Gay 
Bamesli July 1995 
FUNCTION: Transforms linear accelerations from body coordinates to 
earth coordinates and removes the gravity component in the z direction. 
RETURNS : void 
CALLED BY: navPosit 
CALLS: none 


RGR Re ee ik RR RARER KK ERK EERE ER RAKKEEKEEKEERKR KK KKK KEK / 


void 
INS: :transformAccels (double newSample[]}) 
{ 


vector earthAccels; 


newSample[0] -= GRAVITY * sin(posture[4]); 
newSample[1] += GRAVITY * sin(posture[3]) * cos(posture[4]); 
newSample[{2] += GRAVITY * cos(posture[3]) * cos(posture[4]); 


earthAccels = rotationMatrix * newSample; 
newSample [0] 


newSample[1] 
newSample[2] 


earthAccels.element [0]; 
earthAccels.element [1]; 
earthAccels.element [2]; 


ee ee EEE EE EERE ER KERR ERE EEE EEE ER KER EK ® 


PROGRAM: transformWaterSpeed 

AUTHOR:Eric Bachmann, Dave Gay 

Pate ti July 1995 

FUNCTION: Transforms water speed into a vector in earth coordinates 
and returns them in the speedCorrection variable. 

RETURNS: void 

CALLED BY: navPosit 

CALLS: none 


ie rw RRR KEE KK EKER KAREEKEKEREEKREEREEKREKEKEEKEREEKRKEEKEREKRAREREREEREREEKRER / 


void 
INS: :transformWaterSpeed (double waterSpeed, double speedCorrection[]}) 
{ 

double water[3] = {waterSpeed, 0.0, 0.0}; 

vector waterVelocities = rotationMatrix * water; 


speedCorrection [0] 
speedCorrection [1] 
speedCorrection [2] 


waterVelocities.element [0]; 
waterVelocities.element [1]; 
waterVelocities.element [2]; 
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PROGRAM: transformBodyRates 
AUTHOR:Eric Bachmann, Dave Gay 
DATE 1) ds ka 


FUNCTION: Tranforms body euler rates to earth euler rates 
RETURNS: none 

CALLED BY; “nsEeste 

CALLS: buildBodyRateMatrix 


KKK KKKKKKREKEaEKEKKAK KKK HP RII FR ARK KR RISA IK KKK KK SK RIS SIT a i SK 


void 
INS: :transformBodyRates (double newSample[] ) 


{ 
vector earthRates = buildBodyRateMatrix() * &(newSample[3]); 


newSample[3] = earthRates.element [0]; 
newSample[4] earthRates.element[{1]; 
newSample[5] earthRates.element[2]; 


[REE REREERRERKEERREEE KERALA RAK A Ee ee eee 


PROGRAM: buildBodyRateMatrix 

AUTHOR:Eric Bachmann, Dave Gay 

DAT Ee dy aiiiiv ae 

FUNCTION: Builds body to Euler rate translation matrix. 


RETURNS : Yate tCranslactien matrix 
CALLED BY: insPosit 
CALLS: none 


Ree RR Re REE KEE REE ES EER KE eee 


matrix 
INS: :buildBodyRateMatrix () 
{ 


matrix rateTrans; 


float tth = tan( posture (4: 
sphi sin(posture[3]), 
cphi cos (posture[3]), 

cth = cos(posture[4]); 


I 


rateTrans.element [0] [0] iO 
rateTrans.element[0] [1] Cth * sph, 
rateTrans.element[0][2] = tth * cphi; 
rateTrans.element[1][0] = 0.0; 


rateTrans.element[1)][1] = coh; 
rateTrans.element[1][{2] = -sphi; 
rateTrans.element [2] (0) s=2040- 
rateTrans.element(2Z] (ll) -="ephiy scen- 
rateTrans.element (2) [2] =sepne vec. 


return rateTrans; 
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PROGRAM: buildRotationMatrix 
AUTHOR:Eric Bachmann, Dave Gay 
mere: 1) July 1995 
FUNCTION:Sets the body to earth coordinate rotation matrix. 
RETURNS : void 
CALLED BY: insPosit 
insSetUp 
CALLS :none 


RRR ek RRR EERE AEA KEE EERE ARE KERR EKER RKKRKKRR ERE / 


oid 
INS: :buildRotationMatrix() 
{ 
pleat spsi = sin(posture[5]), 
GOcl =" cos (posture (S]), 
sth = sin(posture[4]), 
sphi = sin(posture[3]), 
Geni, = cos (posture|3)]), 
eth = cos(posture([4]); 
rotationMatrix.element[0][0] = cpsi * cth; 
rotationMatrix.element[0)[1]) = (cepsi * sth * sphi) - (spsi * ecphi); 
rotationMatrix.element[0][2] = (cpsi * sth * cphi) + (spsi * sphi); 
Peed tonMatrix.element{1]([0] = spsi * cth; 
peeaetonMatrix.element {i} [i] = (cpsi * ephi) + (spsi * sth * sphi); 
BoeaelonMatrix.element[1)[2] = (spsi * sth * ephi) - (cpsi * sphi); 
rotationMatrix.element[2][0] = -sth; 


rotationMatrix.element[2][1] = cth * sphi; 
mecateronMatrix.element(2)][2] = cth * cphi; 


eR Re ec oo ee ee eS eee ee ee ee ee A ere 


PROGRAM:post multiplication operator * 

AUTHOR:Eric Bachmann, Dave Gay 

DARE Tt July 1995 

PUNGCEION:Post multiply a 3 X 3 matrix times a 3 X 1 vector and 
return the result. 

RETURNS : been, 1 vector 

CALLED BY: 

CALLS : None 


RAKKEKEKERKKEKKKKKEKKKEKKEKKKEKKEEKKEKEKKEKKEKKKKKEKKEKKEKKKKKKRKKKEKKRKKKKKKEEKE / 


vector 
operator* (matrix& transform, double state[]) 
{ 
vector result; 
formtnceie= O10) = 3 144) { 
result.element[i] = 0.0; 


for (int j = 0; j < 3; j+t) ¢{ 
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result.element[i] += transform.element[i][j] * state[j]; 


} 


return result; 


[RRR RRKREREREEKR KEK ERRR ERA A AA ee 


PROGRAM: calculateBiasCorrections 

AUTHOR:Eric Bachmann, Dave Gay 

DATE <11 suily7 199s 

FUNCTION: Calculates the initial imu bias by averaging a number of 
imu readings. 

RETURNS: none 

CALLED BY: insSetup 

CALLS: none 

KHKEKKEEKKEKREKEAKEKERKKEEKKEERA AK AEE ERAEARARE REAR KEE KR ee eee 

void fpeCalculateBiasCorrections(int sig) 

{if (sig == SIGFPE) cérr == “floating point erienwearn 

CalculateBlasCorrections\n:;} 


void 
INS: :calculateBiasCorrections (stampedSample* biasSample) 
{ 

Signal (SIGFPE, fpeCalculateBlasCorrections) ; 


biasCorrecticn | sie — Ue 
biasCorrect1om (4) o= 90 0e 
biasCorrectrven (|S) =—020- 
for (int i = 0; i < biasNumber; i++) { 


while(!saml.getSample(biasSample[i])) {/* */}; 
biasCorrection[3] += biasSample[i].sample[3]; 
biasCorrection[4] += biasSample[i].sample[4]; 
biasCorrection[5] += biasSample[i].sample[5]; 


// Find the average of the biasNumber packets 


biasCorrection[3] = -(biasCorrection[3]/biasNumber) ; 
biasCorrection[4] = -(biasCorrection[4]/biasNumber) ; 
biasCorrection[5] = -(biasCorrection[5] /biasNumber) ; 


//Output the initial bias values 
for(int 3 = 3; j < 6; j++) €{ 
WGOLOny (AS, 1410) - 
Cout << bilasCorrect toni: 


88 


a eR ee a © RRA ERR AAR AEA RREK ERE KERR KKRKEKREREREREREREE 


PROGRAM: applyBiasCorrections 
AUTHOR:Eric Bachmann, Dave Gay 
Babes ti July 1995 


FUNCTION: Applies updated bias corrections to a sample. 
RETURNS : void 

CALLED BY: insPosit 

CALLS: none 


es ee oe ek AREA ERE EKER ER RKKERKEKERERKREKREREKRE / 


void 
INS: :applyBiasCorrections (double sample[]) 


{ 
Static const float biasWght(0.9999), sampleWght(0.0001) ; 


//Calculate updated bias values 


brasCorrection[(3] = (biasWght * biasCorrection[3]) 
- (sampleWght * sample[3]); 

braccorrection[4] = (biasWght * biasCorrection[4]) 
- (sampleWght * sample[4]); 

brasCorrection[5] = (biasWght * biasCorrection[5]) 


- (sampleWght * sample[5]); 


//Apply the bias to the sample 
sample[3] += biasCorrection[3]; 
sample[4] += biasCorrection[4]; 
sample[5]) += biasCorrection[5]; 


// Output the biases 

morgen ) = 3; 3 < 6; J++) { 
goecow, (45, 7+18) ; 
Gout, —< biasCorrection([j]); 
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J. SAMPLER .H 


#ifndef __SAMPLER_H 
#define _ SAMPLER_H 


#include <time.h> 
#include <math.h> 
#include <dos.h> 
#include <conio.h> 
finclude <stdivo/h— 


#include "“toetypes.h" 
f#include “locaticn.h= 
#include "a2d.h" 

#include “compass.h" 


#define MAX SAMPLE NUM 1000 
const int INBUFFSIZE = 512; 


[RRR EE KR ERE RRR RR RR i Pe ee 


CLASS:sampler 

AUTHOR:Eric Bachmann, Randy Walker 

DATE:15 May 1996 

FUNCTION: Formats, timestamps, low pass filters and limit checks IMU, 
water-speed and heading information. 

COMMENTS: This class is extremely dependent upon the specific 
hardware configuration. It is designed to isolate to the INS from 


these particulars. 
KEK KKKEKKKEKEKKEKEKKRKKK KK KKK KKK KEKE KKK KKK KR KKEKKKKEKKKKKEKKERKREKKEKEK RKKKKEKKEKERE / 


class sampler { 
Dubie. 
//Class Constructor 
sampler(): sampleIndex(0), subSampleIndex(0), 
samplePeriod(a2d1l.chent * a2d1l.delta_t * 0.000001) {} 


//Initializes Sampler 
Boolean initSampler(); 


//checks for the arrival of a new sample and formats it. 
Boolean getSample(stampedSample&) ; 


private: 
compass compl; //Compass instance 
a2dClass a2dl1; //A2D instance 


float sample [MAX_SAMPLE_NUM] [8]; 


int subSampleIndex; 


90 


int sampleIndex; 

int sampleCount; 

float samplePeriod; 

Boolean readSamples(stampedSample& newSample) ; 
void filterSample(stampedSample& newSample) ; 
void formatSample(stampedSample& newSample) ; 


void increment (int& index) 
{ if (++index == MAX SAMPLE NUM) index = 0;} 


void decrement (int& index) 
{ 1f (--index < 0) index = MAX SAMPLE NUM - 1;} 


ae 
#endift 
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K. SAMPLER.CPP 


finclide —lostr can 
f#ineclude 2=eenlo en 
#include "sampler.h" 


[EER KREKEEREEREEKEKREREERRERARKKEKRRAK EER ERY EAR ee 


PROGRAM: initSampler 

AUTHOR:Eric Bachmann, Randy Walker 

DATE:12 May 1995 

MODIFIED: 15 May 1996 by Eric Bachmann & Randy Walker 
FUNCTION: Initializes the compass and the A2D module. 
RETURNS -ERUE 

CALLED BY: 

CARES: initCompass(), A2D member functions 


Fee te te Te He ke ee RRR RK RK RR RK RW KK RW RR eR RR Ne ee 
Boolean 
sampler::initSampler() 


compl1.initCompass(); 


a2dl.setRmsoOff (); 
a2dl.setSequencer()j; 
azdl. lockTrigger ()- 
a2dl1.resetFifo(); 
azdl.setrito@. 
aZdl<unleockIrigger():; 
a2d1.setTrigger(); 


reeurn TRUE; 


22 
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PROGRAM: getSample 
AUTHOR:Eric Bachmann, Randy Walker 
DATE:15 May 1996 
FUNCTION: Prepares raw sample data for use by the INS object 
RETURNS: TRUE, if a valid sample was obtained 
CALLED BY:insPosit (ins) 
insSetup (ins) 
CALLS: Get (packetBuffer) 
createSample (sampler) 
formatSample (sampler) 


inLimitSample (sampler) 
Ea Si et eater 6 fT RT REE EEA ERE EARERKEREAKRKKKERKRRAEK ERK / 


//checks for the arrival of a new sample 
Boolean 
sampler: :getSample(stampedSample& newSample) 


{ 
if (readSamples(newSample)) { 
filterSample (newSample) ; 
formatSample (newSample) ; 


meoiinn TRUE 


return FALSE; //Sample packet not available 
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PROGRAM: readeSamples 
AUTHOR:Eric Bachmann, Randy Walker 
DATE 712 May sl2ocG 
FUNCTION: Retrieves all samples of the IMU, water speed, and depth 
that are present in the A2D FIFO until the FIFO is EMPTY. Calculates 


deltalce 
RETURNS: Boolen: TRUE - There were new samples pulled from the FIFO 
FALSE - There were no new samples 
CALLED BY: getSample 
CALLS: getFifoStatus(), getFifoData() 


A a eR ee a I HH KR RR KR RRR RR ee 


Boolean 
sampler: :readSamples(stampedSample& newSample) 


{ 


//Did the FIFO overflow? 


li (a2dl .getFutoStatus() 9 ==) PULL) 
gotoxy (17,200. 
cout << "FIFO Overflowed, execution terminated..." << endl; 
exit(1)-: 


//Does the FIFO have new samples? 
1f (a2dl.getFitestatus() t= EMPTY) { 


sampleCount = 0; //Counts the number of samples taken 


//Empty the FIFO 
while (a2d1.getFifoStatus() != EMPTY) { 


sample [sampleIndex] [subSampleIndex++] = a2d1.getFifoData(); 


//Has it pulled one sample of each channel from the FIFO? 
if (subSampleIndex == 8) { 
subSampleIndex= 0; 
increment (sampleIndex); //set to record next sample 
++sampleCount; 


1f (sampleCount > QO) { 


//calculate time delta 
newSample.deltaT = sampleCount * samplePeriod; 


Gotoxy (ab). 

ied Geen je 

Jgotoxy (1a 19s 

printf (“sampleCount: %*d",sampleCount) ; 
retvrn Thue. 
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else { 
return FALSE; 


else { 


//No new samples 
return FALSE; 


a ee RRR ER EKEREEKREEKKEKKEKEKEKKKEKEEKKKKKEKE K 


PROGRAM: createSample 

AUTHOR: Eric Bachmann, Randy Walker 

DATE:15 May 1996 

FUNCTION: Low pass filters by averaging over all samples received since 
the last sample.. 


RETURNS : vold 
CALLED BY: getSample 
CALLS : none 


ein ee RAKE EERE REE REE REA EA EE EA KEE EK ERE KEKERERKEEKEAKKKKKKE / 
void 
sampler::filterSample(stampedSample& newSample) 
{ 
ber, (int 1 = 0; 1 < 8; i++) { 
newSample.sample[i] = 0; 
int j(sampleIndex); 


beret: = 0; 1 < SampleCount; i++) { 


decrement (j); 


newSample.sample[0] += sample[j][0] / sampleCount; 
newSample.sample[1l1] += sample[j][1] / sampleCount; 
newSample.sample[2] += sample[j][2] / sampleCount; 
newSample.sample[3] += sample[j][3] / sampleCount; 
newSample.sample[4] += sample[j][4] / sampleCount; 
newSample.sample[5] += sample[j][5] / sampleCount; 
newSample.sample[6] += sample[j][6] / sampleCount; 
newSample.sample[7] += sample[j][7] / sampleCount; 


OS 
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PROGRAM: formatSample 
AUTHOR:Eric Bachmann, 
DATE:15 May 1996 
FUNCTION:Converts integers representing voltage readings into 
units which are useable by the INS. 


RETURNS : 


Oud 


CALLED BY:getSample 
CALLS : none 


KEEKKKEK KEK KKK ERR HR KR RII A re ee hn RR ee 


Randy Walker 


//Calls the methods to convert the voltages to real world units 
VoTd 
sampler::formatSample 


{ 


newSample. 


newSample 
newSample 


newSample. 


newSample 
newSample 


newSample 
newSample 


sample[0] 


.sample[1] 
-sample[2] 


sample[3] 


-sample[4] 
-sample[5] 


.sample[6] 
.sample[7] 


(stampedSample& newSample) 


xAccelUnits (newSample.sample[0]); 
yAccelUnits (newSample.sample[1]); 
zAccelUnits (newSample.sample[2]); 


pUnits (newSample.sample[3]); 
qUnits (newSample.sample[4]); 


rUnits (newSample.sample[5]); 


waterSpeedUnits (newSample.sample[6]); 
compl1.getHeading(); 
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L. COMPASS.H 


#ifndef _COMPASS_H 
#define _~COMPASS_H 


fomelude <1ostream.h> 
#ineclude <fstream.h> 
fame lude <conio.h> 


#include "portbank.h" 
#include "toetypes.h" 
fanelude "location.h" 
#include "compBuff.h" 


i ee ie eee NK AKA RAKK KE KER KKK RKKKEKRERERKEKEKK KEKE 
CLASS: compass 
AUTHOR:Eric Bachmann, Dave Gay 
BAPE: 11 July 1995 
MODIFIED: 15 May, 1996 by Eric Bachmann & Randy Walker 
FUNCTION:Reads compass messages from the compass buffer. Checks for 
validchecksum. Corrects heading for megnetic variation. Heading 1s 


continous. There is no branch cut at 360 degrees. 
CR eee ee A ARERR XER XE R AK ERK EERE EEAAE SAREE RARE RR / 


class compass { 
public: 
//Class Constructor 
compass() : port2(COMports.Init(COM2, BYTE(3), b9600, 
NOPARITY, BYTE(8), BYTE(1), NONE, headings) ) 
{ } 


//Initialize currentHeading 
float initCompass(); 


//returns the latest heading 
float getHeading(); 


private: 


//puffer for compass data 
compBuffer headings; 


//Maintains the most recently obtained heading. 
float currentHeading; 


j/7 Wistantiates serial port communications on commzZ 
bufferedSerialPort& port2; 


//calculates the check sum of the message 
Boolean checkSumCheck(const compData&) ; 


//Parses the heading out of a compass message. 
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float parseCompData(const compData&, const BYTE) ; 


// Convert magnetic direction based on magnetic variation. 
float trueHeading(const float); 


// Returns the heading without branch cuts 
float continousHeading(const float); 


ie 


#endif 


M. COMPASS.CPP 


#include <math.h> 
#include “compass.h" 


float 
compass::initCompass () 
{ 
float tempHeading; 
compData rawMessage; 


while ( (headings .Get (rawMessage) ==FALSE) 
1] (checkSumCheck (rawMessage) ==FALSE)) {} 


tempHeading = parseCompData(rawMessage, 'C‘') * degToRad; 
currentHeading = continousHeading (trueHeading (tempHeading) ); 


return currentHeading; 
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ee RA REE RARER EERERKREKKREKRKKEKAEKKEKEREREKERKERERKE 


NAME: getHeading 
AUTHOR: Eric Bachmann, Dave Gay 
DATE:11 July 1995 


FUNCTION: 
Determines if an updated gps position message is available and 
copies it into the input argument 'rawMessage'. If the message 


has a valid checksum and was obtained with atleast three 
Satellites in view, a ‘TRUE' is returned to the caller, 
indicating that the message is valid. 


RETURNS : TRUE, if a valid position message is contained in the 
input argument. 
CALLED BY: navPosit (navigator.h) 


CALLS:Get (buffer.h) 
checkSumCheck (gps.h) 


AAR EAA ARR KR RKARERERRERKERERRERERKEEREKEKKKERE RE | 


ie NWojsha 
compass ::getHeading() 


{ 


float tempHeading; 
Boolean checkSumFlag; 
compData rawMessage; 


if ((headings.Get (rawMessage)) && (checkSumCheck(rawMessage))) { 


tempHeading = parseCompData(rawMessage, 'C') * degToRad; 
currentHeading = 
cont inousHeading (trueHeading(tempHeading) ); 


return currentHeading; 
} 
else { 
return currentHeading; // No updated position is available. 


BY Fe 
asciiToHex(BYTE letter) 


{ 


fie wletter s= "A’) { 


Hoenn Lecter — $A’ 4 1.0) 
} 
else { 

return. Letter — 48) ; 
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[RRR KERKRER KEE RRR EER a eee 


PROGRAM: checkSumCheck 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 

Takes an exclusive or of bytes 2 through 78 in a Motorola format (@@EA) 
position message and compares it to the checksum of the message of the 
message. 

RETURNS: TRUE, if the message contains a valid checksum 

CALLED BY: gpsPosition (gps) 

CALLS: none 


Hk eK IK RK RR FOR I KR AR ee ee 


Boolean 
compass::checkSumCheck (const compData& newMessage) 


{ 


BYTE calChkSum(0) ; 
BYTE mesChkSum(0) ; 


for (int a= 1 newMessage ts |e! ia 


calChkSum “= newMessage[1]; 


mesChkSum = asciiToHex(newMessage[i+l]}) * 16 
+ asciiToHex (newMessage[i+2]); 


return Boolean(calChkSum == mesChkSum) ; 
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PROGRAM: trueHeading 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Convert magnetic direction to true based on local magnetic 
variation. 
RE LURNS : true heading 
@ALLED BY: insPosit 
insSetUp 
CALLS: none 


I RR ee NR KEKE EKER EKER ERREEER EERE KE / 


moa tC 
compass::trueHeading(const float magHeading) 


{ 


meareic double twoPi(2.0 * M_PI); 
double trueHeading = magHeading + RADIANMAGVAR; 


if (trueHeading > twoPi1) { 


trueHeading -= twoPi; 


return trueHeading; 
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[RRR KRKRRERKREREERKKEKRE EAL FERRER REARS AAD Xe eee eae 


PROGRAM : cont inousHeading 
AUTHOR: Eric Bachmann 
DATE: li-July ges 
FUNCTION: Maintains track of branch cuts and returns a continous heading. 
RETURNS : continous true heading 
CALLED BY: insPosit 
insSetUp 
CALES: none 


KKKKKARKKEK KK RE KEK KKK RA ee eee 


Float 
compass::continousHeading(const float trueHeading) 


{ 


Const floats twoli (2303 ieee 
Static int brancheuLt count (a. 
static float previousHeading (trueHeading) ; 


if ((4.71 < previousHeading) && (trueHeading < 1.57)){ 
++branchCutCount; //Went through North in a right hand turn 
} 


else { 
if ((1.57 > previousHeading) && (trueHeading > 4.71)) { 
--branchCutCount;//Went through North in a dere hang Euan 


previousHeading = trueHeading; 


return trueHeading + (branchCutCount * twoPi); 
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PROGRAM: parseHeading 
AUTHOR: Eric Bachmann 
DATE : lil July 1995 
FUNCTION: Parses the heading out of a compass message. 
RETURNS : the message heading as a float 
CALLED BY: insPosit 
insSetUp 
CALLS: none 


CM aaa cesion fae > LR AE AAA KK KAKA REAR AEARAAR KR AEKRAEREREEKKREKKE RE & / 


float 
compass: :parseCompData(const compData& rawMessage, const BYTE key) 


{ 
float dataSum(0) ; 


for(int j = 0; rawMessage[j] != key; j++) {(} 
J++; 
fomint i = 0-> rawMessage[i + j] != '.'; i++) (} 


ewiten (1) f{ 


case 3: 
dataSum = (rawMessage[j] - 48) * 100.0 4+ 
(rawMessage[j+1l1] - 48) * 10.0 + 
(rawMessage[j+2] - 48) + 
(rawMessage[j+4] - 48) * 0.1; 
break; 
case 2: 


dataSum = (rawMessage[j] - 48) * 10.0 + 
(rawMessage[j+l] - 48) + 
(rawMessage[j+3] - 48) * 0.1; 


break; 
case l: 
dataSum = (rawMessage[j] - 48) + 
(rawMessage[j+2] - 48) * 0.1; 
break; 


return dataSum; 
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N. A2D.H 


#ifndef 
#define 


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


//ESP A2D General Global Definitions 
//Base address SEL=1->0x300 & SEL=0->0x100 
//FIFO size (MAX=1000 decimal) 
//Max channels 


#define 
#define 
#define 


//ESP A2 
/ /BASE+0 
#define 
#define 


#define 
#define 
#define 


//ESP A2D Control Register 


= AZDEH 
» AZDEn 


<dos .h> 


<math.h> 


<con1oeh- 
=—Statoon 


estadly ion 
<stdavd.- 


lob 
h> 


<i1ostream.h> 
<fStrean n> 


DEFBASE 
PIPOs 1Z2E 
MAXCHAN 


D Status 
PA gee ONES 
IND SS TAY 
TRG_STAT 


FULL 
HALF 
EMPTY 


0s 10:0 
1000 
0x10 


Register Definitions 


DDDD 
0x10 
0x08 


// 
a 


Ox01 
0x05 
0x06 


//BASE+08h: DDDD DDDD 
//BASE+09h: DDDD DDRR 


#define GATEIJOUT 0x0006 


#define 
#define 
#define 
#define 


#define 
#define 


#define 
#define 
#define 
#define 
#define 


TRG_POS 
Sy Ol Ga Se 
RST_TRG 
INT_EN 


DIFE 
RMS 


Al, 
PRG_SEQ 
ACDC 
SAM_SEQ 
RST_FIFO 


fa 


0x0010 
0x0020 
0x0040 
0x0080 


ae 
// 
vy. 
// 


0x0400 
0x0800 


0x1000 
0x1000 
0x2000 
0x4000 
0x8000 


0001 0000 INTERRUPT STATUS 
0000 1000 TRIGGER STATUS 


// 0000 0001 FIFO FULL 
// 0000 0101 FIFO HALF FULL 
// 0000 0110 FIFO EMPTY 


Definitions 


0000 


0000 
0000 
0000 
0000 


// 
yey 


wes 
ta 
// 
ET 
// 


//ESP A2D Useful Definitions 


#define 


CLRRATE 


OxFFF8 // CLEAR RATE TO 


0000 


0000 
0000 
0000 
0000 


0000 
0000 


0001 
0001 
0010 
0100 
TOGO 


0000 


0001 
0010 
0100 
1000 


1000 


0000 
0000 
0000 
0000 


(1=IRO Pending) 
(1=Triggered) 


(O0T= Biles) 
(10l=Halt Fue 
(1 TO0=Emp ey 


0100 0000 
1000 0000 


0000 
0000 
0000 
0000 
0000 


0000 
0000 
0000 
0000 
0000 


104 


GATE10UT (Always Driven) 
TRIG POS (Trig on +1-) 

TRIG SET (Active LOW) 

TRIG CLR (Active LOW) 

IRQ ENAB (Active HIGH) 

VO0US DLE E Se (1=DIFF Q=sEe 
0000 RMS Mode Ch=Oh O=OFF) 
0000 CAL Mode (1=ON O=OFF ) 
0000 SEQ Mode (1=PRG 0O0=RUN) 
0000" ACDE Mode (1-86 O=AC) 
0000 SAMP/SEQ (1=SEQ O=SAMP) 
0000 FIFO Reset (1=EN 0O=REW) 


HIGHEST RAs 


re ee ee REE EEE KR ARE KA KKK RK KK ee ek ek & & 


// CLASS NAME: a2dClass 

// AUTHOR: Randy Walker 

PoebaATE: 27 March 1996 

// DESCRIPTION: Provides for the software operation of the A2D module. 


a rR EAE RARER ERE KKEEREREEKKKKKKKKKKEEK EE 


fyeless Definition for the A2ZD Class 
class a2dClass { 


public: 


V7Glass Constructor; reads azd.cfg file, initializes hardware 
a2dClass(); 


//Reads a2d.cfg configuration file 
void readConfigFile(); 


//Sets address mapping 
void initSysAddr (void); 


//Initializes the A2D Control Register 
void initHardware(void); 


//Print out the variable ctrlw, for debug purposes 
Voce tne Ctr lw (void) ; 


//Sets the A2D Control Register for Single-Ended mode 
void setSe(void) ; 


//Sets the A2D Control Register for Differential mode 
Void setDiff (void); 


//Loads sequencer memory with channel data 
void setChannel (unsigned seg,unsigned ch,unsigned g10,unsigned g2); 


//Sets sequencer to program mode 
void setProgSeq(void); 


//Sets sequencer to run mode 
void setRunSeq(void); 


//Loads sequencer address counter with number of channels to scan. 
void setCount (unsigned nch); 


//Sets AC or DC Coupling 
void setAcDc (unsigned acdc) ; 


//Prevents triggering 
Vol whecieTrigger (void) ; 


//Allow the triger to function 
void unlockTrigger (void) ; 
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//Toggle the trigger (software triggering) 
void setTrigger (void); 


//Clears the trigger 
void resetTrigger (void) ; 


//Switches in the RMS measurement chip 
void setRmsOn (void); 


//Switches out RMS measurement chip 
void setRmsOff (void) ; 


//Sets the A2D module to sequencer mode 
void setSequencer (void); 


//Sets the A2D module to sampler mode 
void setSamplerRate (unsigned) ; 


//Set GATEIVOUT bit lot veontroemvord high 
void gateloutOn (void); 


//Set GATE1OUT bit of Control worcemloe. 
void gateloutOE£E (void): 


//Sets timer channel 1 to square-wave input 
void squareWaveTimerl (unsigned) ; 


//Initialize the AZD Ciming Using Cimer 2 
VOld Inte Ttiming (ines omeaecdi= 


//Rewind FIFO to beginning of memory 
void resetFifo(void) ; 


//Enable FIFO to acquire data 
vold setFifo(void) ; 


J//Returns the state orecnewniio 
unsigned getFifoStatus (void) ; 


//Returns next data word stored in FIFO 
Signed getFifoData (void) ; 


//Program timer channel 0 to set the desired interrupt rate 
void setIntRate(unsigned intrate); 


//Locksout the interupt request line 
VO1d IMntCOfn( vores. 


//Enables system interuppt request 
Void iantOn (voweoe: 


//Sets the trigger level; trigger level (0=-10V, 128=0V, 255=+10V) 
void setTriggerLevel (unsigned tl); 
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fo seceseral lang of ricing edge trigger 
void setTriggerPosition(unsigned tp); 


V/Caliabrates zero offset error 
vVold zeroOffset (void); 


//Grounds the two differential inputs for zero adjust 
Verd dindkinput (youd); 


//Ungrounds the two differential inputs 
werd £reeinput (void) > 


//Adjust the trimmer on the PGA 
vVold zeroAdjust (void) ; 


fimerchient ; 
unsigned delta_t; 


private: 


unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 


ia 


seme: L 


QO. A2D.CPP 


#include 


Ctr lw 

segqecnt; 

mode_sel; 
mode_acdc; 
samprate; 
sampindex; 
segqaddr [MAXCHAN] ; 
chan [MAXCHAN] ; 
g10[MAXCHAN] ; 

g2 [MAXCHAN] ; 


waa en | 


//ESP A2D Addresses 


unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 
unsigned 


BASE 
1312 
MEM 
STAQ 
COUNT 
TIMERO 
EMG 
TIMER2 
CEMERe 
CNTL 
DAC 


DEF BASE; 
Osc0 07 
0x00; 
Gse0 Ze 
Ox02- 
Ox04; 
0x05 
Ox06; 
0x07 ; 
Ox01S2 
CC; 


iy 
Va 
a 
1) 
ioe 
es 
iy 
// 
a 
ff 
we 


//Number of channels to sequence 
//period between channels 


//Holds A2D Control Register update values 
//Sequence Counter 

//Single-ended or Differential 

Jf PC] DE Coupling 

//Sample Rate in Recurrent Mode 

//Which Channel to Sample in Recurrent Mode 
//Sequencer Address 


//Channel 

//R1O Gain 

//x2 Gain 

BASE I/O ADDR [BASE] ‘o. 
FIFO READ ADDR [00-01] (R) 
SEQUENCER ADDR [00-01] (W) 
STATUS REGISTER LO2] (R) 
SEQUENCER ADDR PTR Oe) (W) 
TIMER 0 [04 ] (R/W) 
TIMER 1 [0S | (R/W) 
ALIOGUDIS. [06] (R/W) 
TIMER CONTROL WORD [07 ] (R/W) 
A2D CONTROL REGISTER [08-09] (W) 
DAC DATA Oc] (W) 
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[ [RE RE RRR EEK ERERE KEK RARER RAR ERE KEK SA ee ee 


// FUNCTION NAME: a2dClass() 
// AUTHOR: Randy Walker 
// DATE: 27 Mare oe 


// DESCRIPTION: Sets defaults, reads a2d.cfg file, initializes address map 


iJ and hardware 
// RETURNS: void 
// CALLS: readConfigFile(), initSysAddr(), initHardware() 
// CALLED BY: Object declaration 
[ [BRR RRR RRERERER RR ERREK ERE AEE RE eee 
a2dClass::a2dClass(void) 
{ 
Ctniw=0- 
seqcent=1; 
mode_se]l=0; 
mode acde=i- 
delta wt - 
ehene =k. 
samprate=0; 
sampindex=0; 
readConfigFile()j; 
initSysAddr (); 
1nitHardware(); 


[ [RRR ERE A RO ee ee 


// FUNCTION NAME: readConfigFile() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Reads the a2d.cfg file and sets variables 
// RETURNS: void 
// CALLS: none 
// CAGLED BY -eazdv class constructor 
[LL RREREEREREREE REAR KR ROR RR Re 
void a2dClass::readConfigFile() 
{ 
FILE *configFile; 
Char junk (ie |: 


Lf ((contigrile >= -fopen (“azdseig”. 74.) s)he n, 
fFprint£(stderr, “Cannot open £1 le, Az Che]. me 
exit(l1); 


fscanf (configFile, "%x%s", &seqent, junk) ; 


if (seqent==0 ||seqent>0x0F){ //seqent must be 1-F (15 max in seg mode) 


cout << “\nsegent out of range wiine+7)- ec Gee ma 
exit(1); 


fscanf (configFile, "%d%s",&mode_sel, junk) ; 
if (mode_sel '=0 && mode_sel != 1){ 
cout << “\nmode sel out ci range in 42D-chG... a 
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eset (1): 


} 

Pecaut (configrile, "dts", &mode_acdc, junk) ; 

feeeimode acdc !=0 && mode_acdce != 1) { 
Sota  Vnmodemacdie OClul Of range in AZD.CFG...\n"; 
exit(l); 


} 


Beeant(conftigrile, "txts", &chent, junk) : 


feeeeieme =— 0 || Ghent > Ox0F){ //chent must be 1-F (15 max in seq mode) 
eo. “\nehncnt ous Of range in A2ZD.CFG...\n"; 
eo clien Gli is 


fscanf (configFile, "%d%s",&delta_t, junk); 


ieee lta t <3 {| delta_t > 8192){ 
Souer=<. “\mdelta_t out of range in A2ZD.CFG...\n"; 
exit(1): 


Mmemrdelta t < 6 && chcnt > 1) { 
2otheeeea o \ncdelta t must be > 6 for chcnt Ss 1...\n": 
exit(l1); 


fscanf(configFile, "%d%s", &samprate, junk) ; 

if (samprate > 7){ 
eoute—<— “\usamprate out of range in AZD.CFG...\n"; 
ere 1) > 


fscanf (configFile, "%x%s", &sampindex, junk) ; 

if (sampindex > Ox0OF) { 
cout << "\nsampindex out of range in A2D.CFG...\n"; 
Sete (1 )ice 


fore (int 1=0;i<seqcnt;1i++) 
fscanf (configFile, "%x%x%x%x", &segaddr[i],&chan[1i],&g10[1],&g2[1]); 
fclose(configFile); 
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[ [RRR RRR RER RE KRE KR EK AKRR  X RR KR OR 


// FUNCTION NAME: initSysAddr () 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets system address mappings 

// RETURNS: void 

// CALLS: none 

// CALLED BY: a2d class veonstructon 

JL [RRR RRERRREREER EEK ARK Aa RR KR A ee 
void a2dClass: ;initsysAddy Gold) 


{ 
//clear BASE 


FIFO &= Ox0F; // FIFO READ ADDRESS [0 Oil jp ex) 
MEM &= OxO0F; // SEQENCER MEM ADDRESS [00,01] (WwW) 
STAT &= OxO0F; // STATUS REGISTER [02] (R) 
COUNT &= OxOF; // SEQENCER ADDRESS PTR [02] (W) 
TIMERO &= Ox0F; // TIMER 0 [04] (R/W) 
TIMER1 &= Ox0F; // TIMER 1 [05] (R/W) 
TIMER2 &= OxOF; // TIMER 2 [06] (R/W) 
TIMERC &= Ox0F; // TIMER CONTROL WORD [07] (R/W) 
CNTL &= Ox0F; // CONTROL REGISTER [08] (R/W) 
DAC &= OxO0F; // DAC DATA [OC] (W) 
//set BASE 

FIFO |= BASE; // FIFO READ ADDRESS [00,01] (R) 
MEM |= BASE; // SEQENCER MEM ADDRESS [00,01] (Ww) 
STAT |= BASE; // STATUS REGISTER [02] (R) 
COUNT [= BASE; // SEQENCER ADDRESS PTR [02] (W) 
TIMERO |= BASE; // TIMER 0 [04] (R/W) 
TIMER1 |= BASE; // TIMER 1 [05] (R/W) 
TIMER2 |= BASE; // TIMER 2 [06] (R/W) 
TIMERC |= BASE; // TIMER CONTROL WORD [07] (R/W) 
CNTL |= BASE; // CONTROL REGISTER [08] (R/W) 


DAC |= BASE; // DAC DATA [OC] (W) 
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hi 
Ag 
// 
// 
cf 
as 
ey 
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// 
ef 


FUNCTION NAME: initHardware() 

AUTHOR: Randy Walker, based on [MAKXUS 95] 

Bere: 27 March 1996 

DESCRIPTION: Sets the A2D Control Register to 0020 and sets the 
data member, ctrlw=0060; initializes the module setup 
for software triggering of the A2D. Programs each 
channel. 

RETURNS: void 

CALLS: outpw() 

GAGLLED BY: aZd class constructor 


ee EE RRR KEKE RE KKEKAKEERREERKEKKEKKEKEKKKEKEREKKKKKEKKRERKK K 


void a2dClass::initHardware (void) 


{ 


eiciow  (CNTL, SET_TRG) ; 
eeaiw = SET TRG|RST_TRG; 


if (mode_sel == 0) 
setSe(); 
else 
setDiff(); 
mem(int 1 = O71 < chent;1++) { 


setChannel (segaddr[i],chan[i],g10[i],g2[i]); 
} 
setAcDc (mode_acdc) ; 
Pare liming (delta _t) ; 
Bereount (chcnt ); 


ner Ce er ae ee NER REE RRR EKER AREER KKK ER KRERKKERKKKEKEKKKKEEK KEK K 


ap 
// 
oe 
7 
// 
7. 
hay 
ee 
// 
V7 


FUNCTION NAME: printCtrlw() 

AUTHOR: Randy Walker, based on [MAXUS 95] 

BATE: 2/ March 1996 

DESCRIPTION: Print A2D control register var, ctrlw. 
The variable is used to set a byte in the 
ESP A2D control register at BASE + 08h/09h 
Used during application code debug 

RETURNS: void 

CALLS: none 

CALLED BY: none 


J [RRR RRKRERKKEEKKKKKEKKEKKKEKEKKEKKEEKRKKKEEKKEEKKKKKRKEKEKEKEKKKKKKEKKEKKKKKEKE 


VotdraZzdeclass::printctrlw (void) 


{ 


Pienaemitctriw: 20d yt", ctriw) >; 
bOopmlincei=-0xs00°1-0x)0-14+) { 
DatitiGe soe Cte we-OxXOF—1) & 1)); 
Tf ((1+1) 44==0) 
ist nde ho ee 
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ie 
ge 
ay 
// 
ie 
// 
ey 
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FUNCTION NAME: setSe() 

AUTHOR: Randy Walker, based on [MAXUS 95] 

DATE: 2/7 March 1996 

DESCRIPTION: Sets ctrlw for single ended mode and writes ctrlw to 
A2D Control Register 

RETURNS: void 

CALLS: outpw() 

CALLED BY: initHardware() 


[LL RRRRERERERERKERKEREKKREKREERERKREREKRRALKEKEREKEK EK RRR RRR Ree 


void a2dClass::setSe(void) 


{ 


Ctriw G=" ole. 
Cutpw (CNT beet r iw); 


[ [RRR RERRERRKREKR EK RREERKREKREKKERE RE RRR RARE RARER RR Rh Re ee 


// 
// 
ie 
ae 
io 
// 
// 
La 


FUNCTION NAME: setDiff() 

AUTHOR: Randy Walker, based on [MAKUS 95] 

DATE: 27 March 1996 

DESCRIPTION: Sets ctrlw for differential mode and writes ctrlw to 
A2D Control Register 

RETURNS: void 

CALLS: oOutpw i) 

CALLED BY: i1nitHardware() 


[RRR RRR RRA ER KARA RE RR i ee ee 


Vold azdClass- -seltbirt(vord, 


{ 


ecrlw |= {Pier 
outpw(CNTL, ctrlw) ; 
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// FUNCTION NAME: setChannel () 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Loads sequencer memory with channel data 
Perlis: progseq(), outpw(), runSeq() 

// CALLED BY: initHardware() 

// VARIABLES: seg - sequencer number 


// ch - channel number 
// g10 - x10 gain value 
/ g2 - x2 gain value 


eager Re RRR EERE RARER ER EERERKEKKKKKKKKKKKKKKEKKKKKKKKEKKEKKEKKKEKE 


Poaceazcaclass::setChannel (unsigned seq,unsigned ch,unsigned gl0,unsigned g2) 


{ 


MmaAsigned d = 0; 


setProgSeq(); // set sequencer program mode 
outpw (COUNT, seq) ; // set sequencer address 


//load sequencer memory 


ea ch=<<c - /7 senanne | 

Cea (gz2<<12); i Jain, x2 

oee= (gl0<<14)-; // gain X10 

outpw (MEM, @) ; // load sequencer 
setRunSeq(); // set sequencer run mode 


TL FRRRREEKKEEKREKRKEKEKEEKKEKEKKEKEEEKEREEKKRKKKKKKKKKKKKKKKRK KK KKK KKK KKK KR KKK KKK 


// FUNCTION NAME: set ProgSeg() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Sets sequencer to program mode 
J/ RETURNS: void 
Vy eerie = Outpw() 
// CALLED BY: setChannel () 
EE RAKEKEEREKEKKKEKKEKKEKRKKEEKREKERKKREKREKEKEKKEKEEREKEKEEKEKKAKKKKEKKEKKK KK 
void a2dClass::setProgSeg (void) 
{ 

ctrlw |= PRG _ SEO; 

oumow (CNTL,ccriw) ; 
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// FUNCTION NAME: setRunSeq() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 Maren 1996 
// DESCRIPTION: Sets sequencer to run mode 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
[ LR RRRERERERERERERERRERERRKERARARAR ARK RRR RR ee 
void a2dClass::setRunSeq(void) 
{ 
ctrlw &= ~PRG_SEQ; 
GUEDW (Chit cen lw... 


[LR R RE RERERERERERERERKEEREES RE eg ee eee 


// FUNCTION NAME: setCount() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

J// DATE? 27 March, 1396 

// DESCRIPTION: Loads sequencer address counter with number of channels 
jig to scan. 

// RETURNS: void 

// CALLS: outpw(), setProgSeq(), setRunSeq() 

// CALLED BY: initHardware() 

// VARIABLES: nch - number of channels to sequence 


[J [RRERERRERKERER ERE RRRERE ERE KR RRR ee eee 


void a2dClass::setCount (unsigned nem) 


{ 
NeW =neh <4. // put in upper nibble 
outpw (COUNT neh)= 9 9/7 oul to reqierer 
setProgSeq(); // reset sequencer 
setRunSeq(); // put it in vunemede 
} 


LL EREEERERREREKREREREEREKEEKEKERKEREEEREAEKEEER EE EE eee 


// FUNCTION NAME: setAcDc() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 

// DESCRIPTION: Sets AC or DC Coupling 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: initHardware() 

// VARIABLES: acdc - holds coupling value 


[ [RRR RRR RRER KEE KRERE ER RRA RK KR KR RR RR a 


void a2dClass::setAcDc(unsigned acdc) 


{ 
iE Veaeadc) 
Cturiw. | =SAGpe- // acdc=1 -> DC 
else 
ctrlw &= ~ACDC; // acdc=0 -> AC 
oucpw (CNTL, ctriw)- 
} 
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// FUNCTION NAME: lockTrigger () 
// AUTHOR: Randy Walker, based on [MAKUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Prevents triggering 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: initSampler () 
SCR R Ra he a A AK RRR EER REER EEE KEKEKEEKKRKKKEKRKREKEKKKK KKK KKH 
void a2dClass::lockTrigger (void) 
{ 
ctrlw &= ~RST_TRG; 
eacpow(CNTL, ctrilw) ; 


ee ERE EREAREEREKEKRKREKKEKKEKEKEKKEEKEKEKKKRKK KE KK 


// FUNCTION NAME: unlockTrigger() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
Pome SCRIEPTION: Allow the trigger to function 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: readSmaple() 
CR a eee ee ee A ARERR REAR RAKE EERE EKER ERE ERK EEK © 
void a2dClass: :unlockTrigger (void) 
{ 
eeriw |= RST TRG|SET_TRG; 
eucpw(CNTL, ctrlw) ; 


ERE ER REAR REE RAKE RE RE RE EKRKEKERERRER EK 


// FUNCTION NAME: setTrigger () 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
J/7MWATE: 27 March 1996 
// DESCRIPTION: Toggle the trigger (software triggering) 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY:readSample() 
ee ee eee ee Aa eA RR KR ERE EAE EK AER ERE ERE RK AERA EKER EK RERER 
void a2dClass::setTrigger (void) 
{ 
outpw(CNTL, ctrlw&~SET_TRG|RST_TRG) ; 
Sutpyw (CNT, ceriw| “SEE TRE|RST TRC) ; 
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// FUNCTION NAME: resetTrigger() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27° Marches oG 
// DESCRIPTION: Clears the trigger 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: readSample () 
[LEER ERE ERE REE EE BER RAL EE Ee RA ER ER ee 
void a2dClass::resetTrigger (void) 
{ 
outpw(CNTL, ctrlw!|SET_TRG&~RST_TRG) ; 
outpw(CNTL, ctrlw|SET_TRE| RSTS0RG):; 


[ [RRR ER RR REK EERE ERR ER RR RR OK ee 


// FUNCTION NAME: setRmsOn() 
// BRUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27° March 1396 
// DESCRIPTION: Switches in the RMS measurement chip 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
[LL BRR REE EERE ERE RR ERA EX RR RR Re ee 
void a2dClass::setRmsOn (void) 
{ 
ctrliw fj sRMSe 
Ouupw (CNTL, cer iw) 


LL RRR RRR ER RR RK IR I ee 


// FUNCTION NAME: setRmsOff () 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Switches out RMS measurement chip 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: initSampler() 
[LEE RRERERERERRERERE EERE KER KEEKE RARE ERK ERX ERR eee 
void a2dClass::setRmsOff (void) 
{ 
ctrlw &= ~RMS; 
OUEDWCCNTE, ctriw)- 
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ee RK EERE ERE KEK KKEKKEKERKKEK ER KEKE 


// FUNCTION NAME: setSequencer () 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Sets the A2D module to sequencer mode 
V7 RETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
ce ERA REE KR ERKERKEREKEE KK KKKEKEKEKKEKKEKERES 
void a2dClass: :setSequencer (void) 
{ 
etriw |= SAM SEQ; 
eucow (CNITL, ctrlw) ; 


ee ERK KAA KEEKKKEERERKKKKKKEKKEKKKKKEKKEKKKKKKKEKKEKKKKEKKEKEKKKERKKEKKEKR KE 


// FUNCTION NAME: setSamplerRate () 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
fyeebeate: 2/ March 1996 
// DESCRIPTION: Sets the A2D module to sampler mode 
V7 eEETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
// VARIABLES: rate - sampler rate 
ee KEK REE EERE RRKEKERKEEKKKEKKEKERKEKEKEKKEREKEKKEKEKEKKKKKKKKKKEKKKS 
void a2dClass::setSamplerRate(unsigned rate) 
{ 
ctrlw &= ~SAM_SEQ; //Set to sampler mode 
Geriw &= CLRRATE; //Clear previous rate to 000 
eer ilw |= rate; //Set new rate 
Sutpw(CNTL, ctrlw) ; //Set Control Word 


ee XE AKA AK KAKREEKRRKERKEKKEAEKKEKKKKERERKREKRKEKREKEKERREEKERS 


// FUNCTION NAME: gateloutOn() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Set GATE1LOUT bit of control word high 
// RETURNS: void 
// CALLS? outpw() 
// CALLED BY: none 
J [RRR EEREEREEEEKKEKEKKEKKKKKKKKKKEEKERKKEKKEEKEKEKKEKEKKKEKKKEKEKRKKKEEKEKEKEKKEKER 
void a2dClass: :gateloutOn (void) 
{ 
Gtriw |= CATELOUT: 
QuupwiCNTL, ctriw); 
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FUNCTION NAME: gateloutoOff () 

AUTHOR: Randy Walker, based on [MAXUS 95] 

DATE: 2/7 March ess 

DESCRIPTION: Set GATE1OUT bit of control word low 
RETURNS: void 

CALLS: outpw() 

CALLED BY: none 


[ [BERRA E EER ERRERER EE REREAD RR ee 


void a2dClass: :gateloutOff (void) 


{ 


ctrlw &= ~GATE10U0T; 
outpw(CNTL, ctrlw) ; 


[ [RRR E REE RREREKEEKRERE ER RAK AK BER RE RN ee ee 


vi 
ah 
ae 
jah 
le 
id 
a: 
Ho 
hee 
as 
// 
ae 


FUNCTION NAME: squareWaveTimerl] () 
AUTHOR: Randy Walker, based on [MAXUS 95] 
DATE: 2/7 March 1996 
DESCRIPTION: Sets timer channel 1 to square-wave input 
RETURNS: void 
CALLS: outp() 
CALLED BY: none 
VARIABLES: dt-micro seconds per period (1 to 8192) 
assuming 8 MHz clock input 
ch-timer channel 1 
ph-local variable 
pl-local variable 


[LR RRRERREREKER ER MEEK KE RRR ee 


void a2dClass: :squareWaveTimerl (unsigned dt) 


{ 


char jsigene ls 


pl = (dt*8)&0xFF; // & Clocks PER aus 
pie= (des) =e 

Guim TIMER. 0x7 G6). // initialize timer 
Gute ( EIMERI pl); J// “AC as. -de lay 


Sutp | TIMER Tbh): f/f Witenes MHZ ne te ec 
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ee te eee he EEK EERE EEKRERKKKEKKKK KKK KKK KKRKKKKKEESE 


// FUNCTION NAME: initTiming() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Initialize the A2D timing using timer 2 
// RETURNS: void 

faeeeLLS: outp() 

// CALLED BY: initHardware() 

// VARIABLES: dt - number of micro seconds (3 to 2730) 


ee ARR KK RARER RK AKARAKEREAEREEKEEEREEEKEKKRAKKEKKEKKEKRKKEKKEKKEKEKKEKKEKKKKKSE 


void a2dClass::initTiming(unsigned dt) 


{ 
char |S gung on le 
pi = (dt*8)&0xFF; // 8 CLOCKS PER uS 
ph = (dt*8)>>8; 
outp (TIMERC, 0xB6) ; // initialize timer2 
emi fi MERZ, p1) ; jp ae US delay 
outp (TIMER2, ph) ; // with 8 MHz clock 
} 


pe AER KAEREKEKRKEKEKERKKEEKKKEAKEREEKKKKEKKKEKKKKKEKKKKKKKKKKKKKKKKKKKKEKK 


// FUNCTION NAME: resetFifo() 
// AUTHOR: Randy Walker, based on [MAXUS 95] code 
V7 eeeate =: 27 March 1996 
// DESCRIPTION: Rewind FIFO to beginning of memory 
// RETURNS: void 
Vy eer GhLsS: outpw () 
// CALLED BY: none 
ie eeeneen ane A KK ERK EEK RAE EKER KEKE EREREERAEE ERK EERE EERE KKKKEERERERE SE 
void a2dClass::resetFifo(void) 
{ 
Gemiw &= ~RST FIFO; 
outpw(CNTL, ctrilw) ; 


a eames eee ee AKO A EKER RR EERE EAE AER ERA AKAERAE KARA ERE RAKE K ERAS 


// FUNCTION NAME: setFifo() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 2/7 March 1996 
// DESCRIPTION: Enable FIFO to acquire data 
J J/ereELURNS: vod 
1/7 2CUS 2) Out pw () 
// CALLED BY: initSampler() 
ye ee ee en A KA Re REE EA RK ER ERR REE EEE RARE 
void a2dClass::setFifo(void) 
{ 
Git iW sip oho le te 
eucpw (CNEL, ctr iw): 
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// FUNCTION NAME: getFifoStatus() 
// RUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Returns Ein Oestatus 

// RETURNS: RETURNS: 6 - empty 

// > = halt foi 
ii iL ae ee 

// CALLS: inpw() 

// CALLED BY: readSample() 

[ [BER RR RRR KRKRKK KER HR KRRKRKEEKKEKRKKKKKK RR RAHA K KEKE KER RRR KKKKKREE EE 
unsigned a2dClass: :getFifoStatus (void) 


‘ 
return (inpw(STAT) &7); 


[ [ER EREREKRR KR KRKEEKEEEEEKKEEEKEKRKERREEKREKHEEEKEREKEEEK EERE KEKE EKER 


// FUNCTION NAME: getFifoData() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Returns next data word stored in FIFO 

// RETURNS: 16bits of data. Lower 12 are A2D data 

// “CALLS a niw () 

// CALLED BY: readSample() 

[ L REREREKEKRKRERKEEEKKKEKKKEKREKRKE RAKE KKKAKKKRKE KAKA KAEEKKKKR REE KEE REE EEE 
Signed a2dClass: :getFifoData (void) 


{ 
return (inpw(FIFO) &0x0FFF) ; //Get data and mask upper nibble 


[ [BREE ER EER EEKRKEEEEEEEEKRKKEKEKKKRERKEEARKKKKKREKKKKKKEEREREEERERER RO EEE 


// FUNCTION NAME: setIntRate() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Program timer channel 0 to set the desired interrupt 
hel rate 

// RETURNS=) void 

// CALLS: 2oulte() 

// CALLED BY: none 

// NARIABLES: intrate-micro secs per period (1 to 8192) 

of assuming 8 MHz clock input 

[ [RR EREREKERKKRKEREERKERKEKKKRKKERKKAKKKKEAREEER ARR Ka ERR EK Ee 
void a2dclass::setIntRate(unsigned intrate) 


( 


outp(TIMERC, 0x36) ; // Set timer 0 to mode 3 
outp(TIMERO, (intrate*8)&0xFF); // Load Least Significant Byte 
outp(TIMERO, (intrate*8)>>8); // Load Most Significant Byte 
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// FUNCTION NAME: intOff() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Locksout the interupt request line 
7 eee TURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
ee RE EEE EEE EERE EREREEREEKKEKKEERKREERKKRKEKKEE SE 
mordsazacClass: :intOff (void) 
{ 
ctrlw &= ~INT_EN; // INT_EN is active high 
Sucew CNT, ctriw) ; 


re Or RR EER RAE ER EA ERR EERE ERR ER KEKEKEKKKKEKKEKKEKEKEEKRKKKEKKEKEKRKKKKKE 


Pee UNCTION NAME: intOn() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

Pye te: 27 March 1996 

// DESCRIPTION: Enables system interuppt request 

// RETURNS: void 

[7 CALLS: outpw() 

// CALLED BY: none 

es ra ee PERE REE REAR EKER ERK KREEKREREKRRRRERKEKEKEEER ES 

vomdealcdCclass: -intOn(void) 

{ 
ctrlw |= INT_EN; // INT_EN is active high 
eucpw (CNTL, ctriw) ; 


eee ee eh ee AA A RR RRA AKA RR EEE ARERR RE KRARE RR ER AREER RA ERR EER SR 


// FUNCTION NAME: setTriggerLevel () 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

PaeeTes 27 March 1996 

// DESCRIPTION: Sets the trigger level 

// RETURNS: void 

(SINUS Telekeye 6 

// s@eLED BY> none 

// VARIABLES: tl-trigger level (0=-10V, 128=0V, 255=+10V) 
eg ee ee 
void a2dClass::setTriggerLevel (unsigned tl) 


{ 
Oui DAC At) 
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[LER RARRRREEREEKEEKREEKEKRARR AR RAK ER KR eee 


// FUNCTION NAME: setTriggerPosition() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 Mancina 976 

// DESCRIPTION: Sets falling or rising edge trigger 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: none 

// VARIABLES: tp: O=ftalling? Vee reing 


[ [BERR EERE ER ER ERRAEEKR ER AER RE RRR RR ee eee ee 


void a2dClass::setTriggerPosition(unsigned tp) 


{ 
ctrlw &= ~TRG_ POS; //Clear previous TRG_POS 
ctrlw |= (tp) ?TRG_POS:0; //Evaluate tp and semecer 
outp (CNT vecuriw) 

} 


[ [RRR RRRREREERREREREARAKR KR ERERE RE RE RR OW ee 


// FUNCTION NAME: zeroOffset() 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Calibrates zero offset error 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
J [RR RR RR RRR RR ee I a eee 
void a2dClass::zeroOffset (void) 
{ 
Wunsi9oned d=07 1702, 4 10 
float sum; 
float OLLTSsSeteru4 |) |4)- 
float bits (45s 
unsigned GgainslUi4|) = (1,10, L0G L0Gy. 
unsigned gains2[4] ie 25 Le 8); 


Sbeser () 
DrinteE("“\n\EGLlO\EGZ\E OFFSET VE Ve bits 


for (qgl0=0-q10<4 -qil0+. ) 
for (g2=0;g2<4;g2++) 
printf (“\n\tsd\Eta\ e+XK - XXX KN Ce x 0a 


setRmsoOff (); 

setAcDc (0); 

setSequencer(); 

INLET imaing (3) - 

setChannel!(0,0,q10,gq2Z); 

GEenaaieue.w)., 

delay (5); //Let new gain values stabilize 


White: kbha t () 


for(qgl0=0 ;gi0<47-qil0re 
for (g2=0 -g2<4 027m 


l22 


setChannel(0,0,g10,g2); 
Grenainowc () ; 
lockTrigger(); 
resetFifo(); 
setFifo(); 
mLockTrigger () ; 
setTrigger(); 
delay (1); 
while (getFifoStatus()!=FULL) ; 
Tock rigger () > 


ben 1—0, sum=0.0;71<FIFOSIZE:1++) { 
d=getFifoData(); 
sum+=(float)d*10/2048; 
} 
Serseterr(gl0] ([g2)]=((float) (sum/FIFOSIZE) -10) / 
(float) (gains10[g10]*gains2[g2]); 


Pres iol) ([o2)=(float) (offisetErr[gl10] [g2]*4096/ 
Z20=cgains lUlq10)] *qains2 iq ]) ; 

} 

elvser ()% 

printf ("\n\tG10\tG2\t OFFSET\t\t BITS") ; 

for (g10=0;g10<4;g10++) { 
for (g2=0;g2<4;g2++) { 
Primers Ves \esd\totl of \ts+04 -16£",q10,q2, 
Stiscebrr|(glolilag2|, birte(ai0) [a2] )= 


} 
freeInput(); 
gereh(); 


[ [BRR RR RRR KEKREKKKREKREKRKEKEEKERERKEEERKKKKKEKKK KEKE KEKEKEKKEEKEKKEKKRKEKEKEKEKEEK 


// SUNCTION NAME: grndinput () 
// AUTHOR: Randy Walker, based on [MAXUS 95] 
(7 DATE: 27 March 1996 
// DESCRIPTION: Grounds the two diff input for zero adjust 
// RETURNS: void 
// CALLS: outpw() 
// CALLED BY: none 
[ [RRR RRR RKRKK KR KKK KKK KR RRR KKKEKKKKKEKKKKKR KK KKK KKK KR KK RRR RK KKK KKK KK KR KK KK 
wWordma2dce lacs: > grind input (void) 
{ 
eelmiw (= CAI 
cubpw (CNG, cer lw) > 
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ee) 
fa 
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yi 
as 
ee 
// 


FUNCTION NAME: freeInput () 

AUTHOR: Randy Walker, based on [MAXUS 95] 
DATE: 27 March 1996 

DESCRIPTION: Ungrounds the two diff inputs 
RETURNS: void 

CALLS: outpw() 

CALLED BY: none 


[LR ERERRERRRERER ERE KERR RARE ER Re ee 


void a2dClass::freeInput (void) 


{ 


ctrlw &= ~CAL; 
Outpw(CNTESecerilwi. 


[ [RRR RRR RE RR RR RE eR eee 


kof 
// 
a 
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FUNCTION NAME: zeroAdjust () 

AUTHOR: Randy Walker, based on [MAXUS 95] 
DATE. 2) (Maren 1996 

DESCRIPTION: Adjust the trimmer on the PGA 
RETURNS: void 

CALLS: outpw() 

CALLED BY: none 


[ [RR RRER EER RARER RR EARRKR ERR KRER RE EE RD eee eee 


void a2dClass::zeroAdjust (void) 


{ 


ime 1; 
unsigned d; 
float sum, offsetErr; 


elrser()] 
printf ("\n \mAbJUST THE “TRIM POT OR 0. 0. Oh Nera a) tnaee, 


setRmsOff(); 
setAcDec (0) ; 
setSequencer(); 
Ine him ees): 


while ( !kbnve ( 
setChannel 


))¢ 

( 
Grnatapue ¢) 

ie 

) 


) 
0,0,3,3); 


(ye; 


‘ 


lockTrigge 
resetFifo ( 
set Fifo(); 
Miilbecie ii teageis () > 
setTrigqger(}) ; 

while (getFifoStatus () !=FULL) ; 
loekTr1gqgqer ()=- 


For (1=0,sum=0.0 -1<PIP@st 42 ae 


d=getFifoData(); 
sum+=(£ loat) d= 107 204e- 
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Sersertmer— | (£loae)y (cum, [FOSIZE)—-10)/8000.0; 


Potiemeeve tHe eMBASURED PC OFFSET IS: %+8.6f\r",offsetErr) ; 


freeInput () ; 


Seccin() ; 
} 
P. A2D.CFG 
8 
0 
i 
8 Pehent < 
e175 
a 
0 
0 0 0 
i i 0 
Z 2 0 
5 és 0 
4 4 0 
5 5 0 
6 6 0 
7 fi 0 
8 8 0 
9 A Z 
A 5 2 
B A 2 
Cc 5 Za 
D A 2 
E 5 yh 
F A 2 


;seqcnt :number_of_seq_addresses_to_load 
~mode sel: 2 DIFF=1_ SE=0 
pnederwacda:  oignal coupling _select__DCc=l  Ac=0 


Number_of_channels_to_sequence_ (hex, _1-F) 


0 


So ©2 2 2 2 2 2 Clo... |. = 


wlelta tt -seehan to Chan Sample rate 1n_microsecs_3-8192 = 1/Hz*chent 
;samprate:_ Sample_rate_in_recurrent_mode__0(fast) -7 (slow) 
;sampindex:_Which_channel_to_sample_in_recurrent_mode 


1 
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APPENDIX B: Serial Port Communications Source Code (C++) 


A. GLOBALS.H 


#ifndef _ GLOBALS_H 
#define _ GLOBALS_H 


// types 

typedef unsigned char BYTE; 
typedef unsigned short WORD; 
typedef unsigned long DWORD; 


#define MEM(seg,ofs) (Gis lb erar”*) MK FP(seq,ofs).)) 
#define MEMW(seg,ofs) (*((WORD far*)MK_FP(seg,ofs))) 


enum Boolean than Se. “CRUE; > 


Pyeeasic bit twiddles 


#define set(bit) (eal oalies 

#define setb(data, bit) data | set(bit) 
feemine Cclro(data, bit) data & !set(bit) 
#define setbit(data,bit) data = setb(data, bit) 
#define clrbit(data,bit) Gata, = clrb(data,; bit) 


7// specific to ports 
#define setportbit(reg,bit) outportb(reg,setb(inportb(reg),bit) ) 
inom ecilrportbit(reg,bit) outportb(reg,clrb(inportb(reg),bit) ) 


#endif 
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B. BUFFER.H 


#ifndef _ BYTEBUFFER_H 
#define _ BYTEBUFFER_H 


#include “toetypes.h" 


#define BYTEBUFSIZE 32 


[EERE REE ERREREEERERERERERERE ERR ER RR RRR RR A RR a 


CLASS: Buffer 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATES July ess 

FUNCTION:Base class for use as a polymorphic reference in the serial port code 


which defines a buffer to be used in serial port communications. 
KREKKKRK KKK RKERKERRE ER ER RR RRE KER AAR BERR RR RO 


class Buffer ({ 
PubITe: 


{//GOnustruceer 
Buffer (WORD sz) : getPtr(0), putPtr(0), size(sz) { } 


//Checks for the arrival cf new Characters in) the boule nem 
virtual Boolean hasData(){ return Boolean(putPtr != 


GeuPrer);"} 


//How much of the Buffer is used (rounded percentage 


//0 - 100) 

Virtual ame capacityUsed(); 

//Read from the buffer 

virtual Boolean Get (BYTE&) = 0; 

//Read to the buffer 

Virtual voud Add (BYTE) = 0; 
protected: 


//Increment the pointer to next position 
Vold inc(WORD& index) { If (4+imndex == size) nd—-=<.— 0. 


//Decrement the pointer 
WORD before(WORD index){ return ((index == 0) ? size - 1 
index - 1);)} 


WORD getPtr, //Location of unread data 
putPtr, //Location to sead@data to 
size; //Size of the buffer in bytes 
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Benines a single buEEer of a specified size for buffering charaters 


received via serial port. 
naan ee RAE RA KARR ERERE ARK A RAK KKK KK RK HK / 


class byteBuffer : protected Buffer { 
miolic: 


PmyeebuUrrertByYTH sz—-ByYTEBUFSLZE) ; 
~byteBuffer() { delete [] buf; } 


Buffer: :hasData; 
Buffer: :capacityUsed; 


eae ter extraction 
Boolean Get (BYTES&) ; 


// buffer insertion 

woke | Add (BYTE ch); 

void Aag(eonst char): 

byteBuffer& operator += (BYTE ch){ Add(ch); return *this; 
} 


protected: 


BYTE* Dur - 
a: 
#endif 


C. BUFFER.CPP 


#include <iostream.h> 
finelude <stdio.h> 


#include "globals.h" 
finelwde “buffer hh" 


Oe eR RE KRRRRRKKERKRKKE RR KEE 


JV f/esurter 


a EKER EKER RRR RE EERE RRR REREKREREEREE 


// Returns the percentage of the buffer in use 


int 
Buffer: :capacityUsed() 
{ 
int cap = (putPtr + size) % size - getPtr; 


Eeeumn: F007 *  %cap +/ ‘size; 
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// byteBuffer 

[ [RRR RR RRIF eee) ee 
//Constructor, instantiates a buffer 
byteBuffer: :byteBuffer(BYTE sz) : Buffer(sz) 


{ 
buf = new BYTE[size]; 


//Reads a charcter from the buffer 
Boolean 
byteBuffer: :Get (BYTE& data) 
{ 
if (hasData() ) { 
data = buf |gebrPerl: 
Ine (geuPer jc: 
return TRUE; 
} 
return FALSE; 


//Writes a character to the buffer and checks for buffer overflow 
void 
byteBuffer: :Add (BYTE ch) 
{ 

but |purPEr] "= sch, 

IG (PuULPEL)- 

if (thasData()) f{ // 1f there's no data after adding data, 

// it overflowed 
cerr << "\nError: bytebutter overt low ua; 


//Writes a character to the buffer 
Zod 
byteBuffer::Add(const char* s) 
{ 
while (*s) 
Add (*s++) ; 
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D. GPSBUFF.H 


ifndef _GPSBUFF_H 
#define _ GPSBUFF_H 


mae lude “buffer .h” 
#include "toetypes.h" 


#define GPSBLOCKS 4 
#define LINE_FEED 10 
#define CARR RETURN 3 


[RRR RRERKKEKKEEKKEEKKEKKKEEKKKKKKEKKEKEKKKKKKKKKKKKKKKKKKKKKKKKRKKKKKEE 

Class buffers GPS position messages via serial port communications. 

Uses a multiple buffer system in which each buffer is capable of holding a 
Single position message. Buffers are filled and processed sequentially ina 
round robin fashion. Messages are checked for validity only upon attempted 


Meads Lrom the buffer. 
RM EERE RRA ERE RE RK KEK REKEKAREAKKEKEKKAKKKKAEAREE / 


class GPSbuffer : public Buffer ({ 
iota lac: 


GPSbuffer (BYTE GPSblocks=GPSBLOCKS) ; 


~GPSbuffer() {delete [] block; } 
Boolean hasData(); // a complete structure is ready 
Boolean Get (BYTES&) {return FALSE; } 
Boolean Get (GPSdata); // get a complete structure filled in 
void Add(BYTE ch); // build the structure as each byte 
// 1s added 
protected: 


Boolean validHeader(GPSdata); // check a block for valid 


// header 
GPSdata *block; // hold the buffered GPS data 
WORD current, )) Sele ecurrens "Cho lock im suse 
last; // the last GPS block in use 
BYE *putPlace; // place to put the next charater received 


e 


#endif 
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E. GPSBUFF.CPP 


#include <icstreanm hn 
fine lIude =stdlorm- 


f#include “"“epcbut ivi. 


[RRR KR RRR RR ee ee eee 


PROGRAM:GPSbuffer (Constructor) 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Allocates message buffers, indicate that no data has 
beenreceived by equalizing current and last and set position 

into which initial character will be read. 

RETURNS: neoening- 

CALLED BY:navigator class (nav.h) 

CALLS :none. 


Sada a ca a Ea Go a 8 8 Go a al Cala a Sah Ue ES es SS ES US ESET a ag Es Pe ie PPP ee ey 


GPSbuffer: :GPSbuffer (BYTE GPSblocks) 
curren: (0) last (iy 
Buffer (GPSblocks) // Call to base class constructor 


block = new GPSdata[GPSblocks]; //Create an array of GPSdata 
//elements 

putPlace = &(block[current][0]); //Set the place for the 
// fLivstrcharacter 


IZ 


ore ee RRR ER KK REAR EKER ARK AAR AKKREEEKEKEKKK KKK KKK KKKKKKK KEKE 


PROGRAM :GPSbuf fer: :Add 

AUTHOR:Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Interrupt driven routine which writes incoming characters into the 
gps buffers 

Pee wRNS nothing . 

CALLED BY: interupt driven by bufferedSerialPort 

CALLS :none. 


Rh ee REE EEE KEK AKA AREER EKER AKERS / 


vo 14d 
GPSbuffer::Add(BYTE data) { 


static BYTE lastChar(data); //Holds last for <cr> <lf> detection 
static Boolean 1fFlag = FALSE; //True when message end is detected 


//Is a new message starting? 


me (lirtlag && (data == '@')) { 
last = current; // Set last to buffer with newest message. 
ime (ellrrent) ; jp oes CUubrenbeeemene next buffer 


// Set putPlace to the beginning of the next buffer. 
pueEPlace = &(block[current][0]); 
lfFlag = FALSE; // reset for end of next message. 


*putPlace++ = data;// Write character into the buffer. 


//Has the end of a message been received? 
if ((lastChar == CARR_RETURN) && 

(data == LINE FEED)) { 

meeieag =) TRUE - 
} 


MWeastChar = data: //Save last character for <cr> <lf> detection 
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PROGRAM:GPSbuffer: :Get 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION:Checks to see if a 
new message has arrived, copies it into the 
input argument data and returns a flag to indicate whether a 
newmessage was received. 
RETURNS: TRUE, 1f a new valid position has been received. 
FALSE, otherwise 
CALLED BY :navPosit “(nav epp) 
initializeNavigator (nav.cpp) 
CALLS :GPSbuffer: :hasData 


I NM Mh Ee ie SS REN aS I oI) oS UE ToT) EE Not eS oS OS ah A RA SE OS TES RS NS OSES GP 


Boolean 
GPSbuffer::Get(GPSdata data) 
{ 
// Has a new valid message been received. 
if (hasData()) { 
// Copy the message out of the buffer. 
memcpy (data, block + last, GPSBLOCKSIZE) ; 
// Indicate that this message has been read. 
last. = (Ccureent. 
Feturi RweE 
} 
else { 
reeurn FALSE; 
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PROGRAM: GPSbuffer::hasData 

AUTHOR:Eric Bachmann, Dave Gay 

Bere s))! July 1995 

FUNCTION:Determines whether a new message has been received and 
checks to see if it has a valid header. 

RETURNS: TRUE, if a new valid message has been received. 

CALLED BY: GPSbuffer::Get (buffer.cpp) 

CALLS: validHeader (buffer.cpp) 


REA RAEEEER ER EEK EEK ERKREKEK KKK KEK & / 


Boolean 
GPSbuffer: :hasData () 
{ 


// Has a new message with a valid header been received 
eet last != current) { 
1f (validHeader(block[last])) { 
Becwin. IRUE: 
} 
else { 
return FALSE; 


} 
mecurn FALSE; 


LRA KERKKKKEKKKEKEKEKKEKEKEKKEEKKEEKEKAEKEKKEKKEKEKEKKKEKKKKEKERKEKKKEKEKKEKKSK 


PROGRAM: validHeader 

AUTHOR:Eric Bachmann, Dave Gay 

Dewees ll July 1995 

FUNCTION:Checks to see if a message has the proper header for a 
Motorola position message. (@@Ea) 

RETURNS: TRUE, if the header is valid. FALSE, otherwise. 

CALLED BY:GPSbuffer::hasData (buffer.cpp) 

CALLS:none. 

COMMENTS : 


ei eee ee EE REE EERE EAE AREER RKERRXEREXKR XE / 


Boolean 
GPSbuffer::validHeader(GPSdata dataPtr) 


{ 


1f ((dataPtr[0]) == '@') && (dataPtr[1] == '@') && 
(dataPtr PZ] ==*"E") && 
(dataPtr[3] == ‘'a')) { 


Beeurie LRUE- 
} 


else { 
return FALSE; 
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F. PORTBANK.H 


#ifndef _ PORTBANK_H 
#define __PORTBANK_H 


#inelude "ser ilaleen. 
#inelude "buffer ih ' 


[RRR REREREREERRERRALAS ER RAK ARR ee 


CLASS :portBank 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: tie dulye2os 

FUNCTION:Manages up to four bufferdSerialPort instances. 


BRR KE KR KR IK RE RRR KR RA KL ite a a ae ae Ce | 


class portBank ({ 
public: 
DOLtGBank (+ 
~portBank () { cleanup(); } 
bufferedSerialPort& Init(COMport portnum, BYTE irg, 
BaudRate, ParityType, BYTE wordlen, BYTE stopbits, 
handShake, Buffer&); 


void cleanup(); 


friend IntHandlerType COMlhandler, COM2handler, 
COM3handler,COM4handler; 


protected: 
bufferedSerialPort* ports[4]; 
Bp 
extern portBank COMports; 


#endif 
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G. PORTBANK.CPP 


foe lude <10OSsStream.h> 


Tome lude “serial .h" 
Pomelude "“buffer.h* 
#include "“portbank.h" 


pPoreBbank COMperts: 


J// Constructor, sets up array of ports 
portBank: :portBank() 
{ 
Meme (ant 1 = 0; 1 < 4; i++) 
eres | 1] <= -0> 


// Resets all ports to the original parameters 
void 
portBank: :cleanup () 
{ 
Pome tit =O; 1<4> 14+) 
delete ports[i]; 


// Initializes a serial port based up the input arguments 

bufferedSerial Port& 

portBank::Init(COMport portnum, BYTE irg, BaudRate baud, ParityType parity, 
BYTE wordlen, BYTE stopbits, handShake shake, Buffer& buf) 


apaye iidesce = Seren morenum) = > 
1f (ports [index] ) 
delete ports [index] ; 
ports[index] = new bufferedSerialPort(portnum, irq, baud, 
parity,wordlen, stopbits, shake, buf); 
return *ports[index] ; 


// Three specific interrupt handlers which map each interupt to ///the 
Peeper ESR. 
void 
interrupt 
COMihandler(...) 
{ 
COMports.ports[0]->processInterrupt () ; 
EOL: 


void 

interrupt 

COM2handler(...) 

{ 
COMports.ports[1]->processInterrupt (); 
HOLT; 
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void 

interrupt 

COM3handler(...) 

{ 
COMports .perts |Z] —-Sprecessinterrupe (ye 
EOL; 


VO1d 

Ineer rape 

COMAnhanditer (29s) 

{ 
COMports .ports[3]->processInterrupt(); 
Od 


H. SERIAL.H 


#ifndef _ SERIAL _H 
#define _ SERIAL_H 


“dos he 
<Sipeio > 
Wo lbobatls- i) 
seb Gea jg lal 


#include 
#include 
#include 
#include 


// user defines 


#define ALMOST_FULL 80 // © £uULl) Go Gurpneeore ae 
// leave the following alone - hardware specific 

enum COMport (COML=1,,- "COM2, COMa eon 

enum BaudRate {b300;, bizZ00, b24d00 7 b2s00  boscu 
enum ParityType {BRROR=—1 NOPAR TTY Obese iin. 

enum handShake {NONE, RTS_CTS, XON_XOFF}-; 

enum Shake {OnE son 

enum interruptType {rx_rdy, tx_rdy, line_stat, modem_stat}; 
#define BIOSMEMSEG 0x40 

#define DLAB 0x80 

#define IROPORT OxZi 

#define EOI OUT port hb (0xZ OF 0320) 

#define COMlbase MEMW (BIOSMEMSEG, 0) 

#define COM2base MEMW (BIOSMEMSEG, 2) 

#define COM3base 0x03e8 

#define COM4base 0Ox02e8 

#define TX (portBase) 

#define RX (portBase) 

#define IER (portBase+1) 
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#define IIR (portBase+2) 
#define LCR (portBase+3) 
#define MCR (portBase+4) 
#define LSR (portBase+5) 
#define MSR (portBase+6) 


#define 
#define 


LO_LATCH (portBase) 


a AREER ERR ER RKEKREREKAKKEEKKEKEKKKEEKEK EK 


CLASS:serialPort 


AUTHOR:Frank Kelbe, Eric Bachmann, 


DATE:11 July 1995 
FUNCTION: 

class serialPort { 
peuplic: 


serialPort (COMport port, 


HI_LATCH (portBase+1) 


Dave Gay 


Defines a simple serial port. 
RONG ee cic Cea ARR EEK AR KEKKEREKKREAKKEKEREEREKKE / 


BaudRate, ParityType, BYTE wordlen, 


BYTE stopbits, handShake) ; 


Send(BYTE data); 
Get (BYTE& data); 


Boolean 
Boolean 


inline Boolean dataReady (); 
Boolean statusChanged() 


(Reelin bes lean ((itoortbit(MSR,0) || ifportbit (MSR,1))); 


// the rest are only if handshake is specified as RTS_CTS 


inline Boolean 
imlenme Vo cl 


ieeoruDole (WORD, BYTE); 
toggle (Shake&) ; 
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Boolean TeCneom() (SrecuUmn Ifportbre (MSR 4) - 
Boolean isDSRon () ereerlia lipo rtione (MSR, 5); 
wevlisl set DTRon () {Seu porebit (MeCR-0) >. } 
void set DTRofE£ () { (Clipertaru (Mer. 0). -} 
vio1d EegqlebpiTR() ; 
wold setRTSon () { setportbit(MCR,1); } 
void setRTSoff() {Clr porebie MCR.) >. > 
void toggleRTS(); 
protected: 

WORD portBase; 
handShake ShakeType; 
Shake DTRstate, 

RTSstate; 


} 
} 


) 


// this is the type for a standard interrupt handler 
typedef void interrupt (IntHandlerType) (...); 


[EER RKERERARE ERA ES KERR AES RR Re 


CLASS: bufferedSerialPort 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 1 iaduly 123s 

FUNCTION: Defines a buffered serial port which is interrupt driven 


on receive, and buffers all incoming characters in the specified buffer 
EERE REREAD RS SR RO ee 


class bufferedSerialPort : public serialPort { 
public: 


bufferedSerialPort (COMport portnum, BYTE irg, BaudRate, 
ParityType, BYTE wordlen, BYTE stopbits, handShake, Buffer& ); 


~bufferedSerialPort (); 

Boolean Get (BYTE& data) ; // buffered version 
protected: 

Buffer& bust; 


BYTE irgbit,//Value to allow enable PIC interrupts for COM port 
origirg,//keep the original value of the 8259 mask register 
comint; 


vVold processinterruprt.); // buffers the incoming character 


IntHandlerType *origcomint;// keep the original vector for 
//restoring later 


// this allows the actual handlers to access processInterrupt () 
friend IntHandlerType COMlhandler, COM2handler, COM3handler, 
COM4handler; 


#endif 
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I. SERIAL.CPP 


#include <iostream.h> 
time luide <stdio.h> 
Home lide "serial .h* 


I EPR KAKKKK EERE KK KKK K KKK KR KAKA EK 


Usage Note: Because of the interrupt handlers used, you MUST call your 
MieereredserialPort objects porti, port2, or port3, so the 


right handler gets called and can properly service the interrupt. 
Ah RARER EEE RR ARERR EREKKKKKRK KK EEEKEKE / 


enn fe re A RK KK KARR ERK KKKRKEKKERKKEKKEKKKKKKKKKEKKKEE 


PROGRAM: serialPort (Constructor) 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
PUNCTION : 
Initializes the one of the Serial Ports. 
1) Determines the base I/O port address for the given COM port 
2) Sets the 8259 IRO mask value 
3) Initializes the port parameters - baud, parity, etc. 
4) Calls the routine to initialize interrupt handling 
5) Enables DTR and RTS, indicating ready to go 


Pe eee ee ee eR OE RRA EEE RRR ERE EEK KERR KR KEKE RE ARKKKEKKEKE / 


serialPort::serialPort(COMport port, BaudRate speed, ParityType parity, BYTE 
wordlen, BYTE stopbits, handShake hs) 
DTRstate(off), RTSstate(off), ShakeType(hs) 


switch (port) { 


case COM1: portBase = COMibase; 
break; 
case COM2: portBase = COM2base; 
break; 
case COM3: portBase = COM3base; 
break; 
case COM4: portBase = COM4base; 
break; 
} //switch 
const WORD pandeton | = {0x1e0, 0x60, 0x30, O0xlée, O0xC); 
// Change 1 
Suporte Luk, 0) > // disable UART interrupts 


(Jered) mnm@ort > (LS): 

(volta) iI npertb( MSR) ; 

(vo ld jmnporkio( lik) ; 

(Vota jinpertbp (RX) - 

outportb(LCR, DLAB);//set DLAB so can set baud rate(read only 
i) POL) 

outportb(LO_LATCH, bauddiv[speed] & OxFF) ; 

outportb(HI_LATCH, (bauddiv[speed] & OxFF00) >> 8); 

setportbit (MCR, 3); (7 Ete OUlZ oR 


BYTE opt = 0; 
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1f (parity != NOPARI iw a. 
setbit (opt, 3); // enable parity 
if (parity == EVEN) 7/7 set even parity ble ye 2 seccdmmlca oeoleme 
setbit (opt, 4); 
} 
// now set the word length. len of 5 sets both bits 0 and 1 to 
// 0, 6 sets to O17) J8to 10 ana acer 
opt |= wordlen-5; 
opt |= --stopbits << 2; 
OUtpOrth  LUR ares 


if (ShakeType == °RUSseTs) { 
setDTRon () ; 
setRTSon(); 


[RRR ERERERRER RE RRR RK ER KR RR ee ee eee 


PROGRAM: Get 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Gets a byte from the port. Returns true if there's one there, and 
fills in the byte parameter. If theres no character, the parameter is left alone, 


and false is returned 
RERERERRERERERR ELE EARNER RR RRA Re RRR RAR SS RR ee ee 


Boolean 
serialPort::Get (BYTE& data) 
{ 
1f (dataReady () ) { //make sure there's a char there 
data =| iInpereb (kh): //read character from 8250 
return TRUE; 
} 
else 
return FALSE; 
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PROGRAM: Send 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Outputs a single character to the port. Returns Boolean 


status indicating whether successful 
ee a Ra aa ee Se RARE KARE REE RK RE KKKKEKEKERERE / 


Boolean 
serialPort::Send(BYTE data) 
{ 
Comer (i tpoortbit(LSR, 5) )) // wait until THR ready 
; // NULL statement 
switch (ShakeType) § { 
case NONE: 
SUEPoOreED(TxX,data) ; 
return TRUE; 
ease RTS CTS: 
it (isCTSon() && i1sDSRon()) { 
Gulperen(l*,data): 
return TRUE: 
} 
else return FALSE; 
case XON_XOFF: 
default: 
// add this later if needed 
break; 


} 
Peturn FALSE: 
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PROGRAM: dataReady 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 1) July soos 

FUNCTION:Checks port to see if a character has arrived. 


KK KKK KH KR KERR RK KE KH ERB RE HK RR RRR Re ee ee 


inline 
Boolean 
serialPort::dataReady () 
{ 
f= 
LEV{ Lepore bie (Ske) 
Cerr <<"“\nOverrun Error n-- 
} 
if “(Pi poert 1 t(ueh. 2) eee 
cerr <<"\nParity Errom wn. 
} 
ge | alse cloperalantia Sik) 1) Me 
cerr <<" \nrraming BErer me 


ye! 
YTeluUrn PVEVertebie (joke ie 


[ERR RREREEKER EAA KE REAR RR A RA er ee a 


PROGRAM: ifportbit 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE -2) Jil elo os 

FUNCTION: 


KK He RK He KK RII RI KOR RIS TR KR TR ERE 


inline 
Boolean 
serialPort=:1f£portbit (WORD veg, by liebe, 
{ 
BYTE on = 2nporkeba( req); 
on &= set (bit); 
return Boolean(on == set (bit) ); 
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PROGRAM: toggleDTR 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: toggles Data Transmit Ready if RTS_CTS is off 


Ree GRR og i ERK KEK AREER KAR KKK KAA KE A KK / 


void 
serialPort: :toggleDTR () 
{ 
if (ShakeType != RTS_CTS) 
return; 
if (DTRstate == off) 
setDTRon(); 
else 


Set DP TROLE ( ) > : 
toggle(DTRstate) ; 


[BREE KKKEKKKEKKKEKKKKEKKKEKKKKKKKKKEKKKEKKKKKKKKKKKKKKKKKKKKKKKKKKKK 


PROGRAM: toggleRTS 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

pares 1i July 1995 

FUNCTION: toggle Ready to Send (RTS) if RTS_CTS is on. 


ene 0 nn Tw kK RRA AKERS RRR RR ERK RRR R RAK R KEKE K RRR K / 


void 
serialPort::toggleRTS() 
{ 
ie (ShakeType != RTS CTS) 
Eeturn; 
He {RTSstate == off) 
setRTSon (); 
else 


SetRTSoff (); 
toggle(RTSstate) ; 


[ee EK RR KK RRR RAKE RRR RRR RRR KEK KRAKERREKKEKKREKEKARKKEKEKEKKKEKKREE 


PROGRAM: toggle 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: toggles value of the input variable 


rer ee ee he RT KKK RRR RRR ARERR E RAKE KEK KREEARE KEKE REKKRAEEKEKEEEE f 


inline 
void 
serialPort::toggle(Shake& h) 


ape a == Oft ) 
lg = iejays 


hn =s Orn. 
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77 bufferedSerialPort 


[ [RR RKREREEREREKRERER KR ERE RR RR ER KK RK RRR RR ee 


[ RRKRRKRREK HERR EREE KERR EX EAA RA A KK RRR ee 


PROGRAM: bufferedSerialPort (Constructor) 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11d ies 
FUNCTION: 
Initializes the interrupts for the Serial Port. 

1) takes over the original COM interrupt 
2) sets the port bits, parity, and stop bit 
3) enables interrupts on the 8250 (async chip) 
4) enables the async interrupt on the 8259 PIC 


RREEKKEREK RK RR KEKRE RRR RAR ED OR eee 


bufferedSerialPort: :bufferedSerialPort (COMport portnum, BYTE irq, 
BaudRate baud, ParityType parity, BYTE wordlen, 
BYTE stopbits, handShake hs, Buffer& b) 
serialPort (portnum, baud, parity, wordlen, stopbits, hs), 
buf(b), irabiete irq) 7 comiie Geert os) 


if (ShakeType == RTS_CTS) ( // turn 2t Cit ftest, becanseeee 
// was enabled 
SeLpTReti) // in the base class 


setRTSoff(); 
} 


origcomint = getvect (comint); //remember the original vector 


switch (portnum) ( 


case COMI: 
setvect (comint,COMlhandler); //point to the new handler 
break; 

case COM2: 
setvect (comint,COM2handler); //point to the new handler 
break; 

case COM3: 
setvect (comint,CcOM3handler); //point to the new handler 
break; 

case COM4: 
setvect (comint, cCOM4handler); //point to the new handler 
break; 

} 

(ae setportbit (MCR, 3); 7 tela OUT Sem 
disable(); // disable all interrupts - critical section 
setportbit(lER,rxeray.)- /fenable ints on receive only 
Orlgirg = 1nporteb (TROLORT //remember how it was 
clrpoertbit (LROPORT-i1vabie //enable COM ints 
if (ShakeType == RTS_CTS) ({ 

set DTRon (): 
setRTSon(); 
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enable(); 
EO > 


re ARERR RAK AR KKK RRRKKRAEEKREKEREREKEEKKKKKKKKKKKKKKKKKKKKEKK 


PROGRAM: ~bufferedSerialPort 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
PATE: 11 July 1995 
FUNCTION: 
Resets the interrupts. 1) disables the 8250 (async chip) 
2) disables the interrupt chip for asyne int 
3) resets the 8259 PIc 


RR RMR a ee AEA ARK A AR KKAEK ERE KEKE REEKEAKERE f 


bufferedSerialPort::~bufferedSerialPort () 


{ 
Seevocte (comint, orligcomint) ; //set the interrupt vector back 
Silewporeo( LER, 0) - //disable further UART interrupts 
outportb(MCR, 0) ; JfVEUcmeeverything oft 
Simeeateo( PROPOR), Origird) ; 
EOL; 


ee AK KEE KEELER EE KRKERERKER KEK EKEKKKKEKEREKEKKKKRKKKK KS 


PROGRAM: Get 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
Poteet July 1995 

FUNCTION: Calls Get base on buffer type 


Pe eeaenee eee A A ES EEE ARERR EAR ERE ERK EKER EERE ERE RK / 


Boolean 
bufferedSerialPort::Get (BYTE& data) 


{ 
return buf.Get (data); 


) ae KARR RR RR RK ERK EKER K RK AKKKEKRKEREREKEKERKERKKKKREKAKKKK EX 


PROGRAM: processInterupt 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: Calls the ISR based upon buffer type 


a eae A A nn KA AEA KARA RRA KK EKARK EK  / 


Voud 
bufferedSerialPort: :processInterrupt () 
{ 
1f (dataReady ()) { J/Make SUreCEnere'’s 4 Ccharvenere 
Bulb data =] inporto( RX) ; J /Vead. Character trom 3250 
buf .Add(data) ; 
if (ShakeType == RTS_CTS && buf.capacityUsed() > ALMOST_FULL) 
setDTRoff£(); 
} 
} 
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PROGRAM: showPorts 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: Prints interupt vector addresses 


HHH HH He RK II HK AK KK KK AR AR SRK RT ee 


int 
showPorts() 
{ 
BYTE* p = ( BYTE”) COMZ base, 
pot) 55 
fprintt (stderr, =: wy (p++) > 


fprintf(stderr, "@X\n", *p++); 
forint£ (stderr, “IROPORT = 6X") tnpores 1ROrerT a: 
return 0; 
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J. COMPBUFF.H 


#ifndef _ COMPBUFF_H 
#define _ COMPBUFF_H 


#include "buffer.h" 
#include "toetypes.h" 


#define COMPBLOCKS 8 
#define LINE _FEED 10 
#define CARR RETURN 13 


a KERR EERE EKER ERREKRKEKRKKKREKEKRERKKEKKKKEKKEKREK 


Class buffers COMPASS messages received via serial port communications. 
Uses a multiple buffer system in which each buffer is capable of 


holding a 


Single message. Buffers are filled and processed 


sequentially in a round robin fashion. Messages are checked for validity 
only upon attempted reads from the buffer. 


A eee eee ERR EE EAE EEE EER ERE EERE EERE RE EERE KRE EER KKEKKERER SE | 


class compBuffer : public Buffer ({ 


public: 


Comepurrer (BYTE compBlocks=COMPBLOCKS) ; 
~compBuffer(){delete [] block; } 


Boolean 
Boolean 
Boolean 
void 


protected: 
Boolean 


compData 
WORD 


BYTE 


#endif 


hasData(); // a complete structure is ready 

Get (BYTE&) {return FALSE;} //Satisfy inheritence requirements 
Geclcompobata): 99/7 get a complete structure filled in 

Add (BYTE ch); // build the structure as each byte is added 


validHeader(compData); // check a block for valid header 


thlock. // Pointer to array of compass messages 
Current, // the current comp block in use 

last- // the last comp block in use 

*putPlace; // place to put the next charater received 
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K. COMPBUFF.CPP 


#include <iostream-h> 
Fine lide <stdio-n-— 


tinclude “compbutti sa. 


[LERRERRERERREREREERERE ERR RAAT A RR KR Re ee ee 


PROGRAM: compBuffer (Constructor) 

AUTHOR:Eric Bachmann, Randy Walker 

DATE:28 April 1996 

FUNCTION: 

Allocates message buffers, indicates that no data has been 
received by equalizing current and last and sets the position 
into which initial character will be read. 

RETURNS nothing. 

CALLED BY:compass class (compass.h) 

CALLS: none. 


eek Re eK eR Ke KK He KR RRR RE IR KR ee eee 


compBuffer::compBuffer (BYTE compBlocks) : 
current (0) laste (oye 
Buffer (compBlocks) //Call to constructor of the base class 


block = new compData[compBlocks]; // Create an array of message buffers 
putPlace = &(block[current][(0]); // Set position for first character 


150 


[BRR RR RR IR RR I II RO RR RR IR IK OR IRR RR kk 


PROGRAM: compBuffer: :Add 

AUTHOR:Eric Bachmann, Randy Walker 

DATE:28 April 1996 

FUNCTION: 

Interrupt driven routine which writes incoming characters 
into the coompass message buffers 

RETURNS: nothing. 

CALLED BY: interupt driven by bufferedSerialPort 
CALLS:none. 


ae ee ARERR AE AKER EA ERA RAE ALARA AEA AA KARE RA ERE EK A / 


void 
GempBurtfer::Add(BYTE data) { 


static Boolean 1fFlag = FALSE; //True, if message end detected 
static int messageCount(0); // Counts characters in current message 


//Is a new message starting? 


Pie tthlag && (data == '$')) { 
last = current; // Set last to buffer with newest message. 
ne (Current) ; // Set current to the next buffer 


// Set putPlace to the beginning of the next buffer. 
putPlace = &(block[current] [0]); 
lfFlag = FALSE; // reset for end of next message. 


*putPlace++ = data;// Write character into the buffer. 
messageCount++; 


//Has the end of a message been received (<cr><lf>)? 
teaata == LINE FEED) { 

dtrlag = TRUE; 

} 


Lil 
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PROGRAM: compBuffer::Get 

AUTHOR:Eric Bachmann, Randy Walker 

DATE*+28 Api f276 

FUNCTION: 

Checks to see if a new message has arrived, copies it into the 

input argument data and returns a flag to indicate whether a new 
message was received. 

RETURNS: TRUE, if a new valid position has been received. 
FALSE, otherwise 

CALLED BY: compass.cpp 

CALLS: compBuffer: :hasData 


ER KKEKK EEE KK ERR EKE KR RK EK KEES ER AK RR RR ee 


Boolean 
compBuffer::Get(GPSdata data) 
{ 
// Has a new valid message been received. 
if (hasData() ) { 
// Copy the message out of the buffer. 
memcpy (data, block + last, COMPSIZE) ; 
// Indicate that this message has been read. 
last = current - 
return TRUE; 
} 
else { 
return FALSE; 
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PROGRAM: compBuffer: :hasData 

AUTHOR: Eric Bachmann, Randy Walker 

DATE:28 April 1996 

FUNCTION: 

Determines whether a new message has been received and checks 
to see if it has a valid header. 

RETURNS: TRUE, if a new valid message has been received. 

CALLED BY: compBuffer: :Get 

(CUNT S. validHeader (compBuffer.cpp) 


ee ear a EEA EAE EKER CAE ERA KER RAKE KEREKEEKE / 


Boolean 
compBuffer: :hasData() 
{ 
if ((last != current) && (validHeader(block[{last]))) { 
Petllmen TRUE: 
} 
else { 
return FALSE; 


} 


EE REAR ERR KX KERR RAKKKKEKEEKKKKKKK 


PROGRAM: validHeader 

AUTHOR:Eric Bachmann, Randy Walker 

DATE:15 May 1996 

FUNCTION: 

Checks to see if a message has the proper header for a compass 
message. (SC) 

RETURNS: TRUE, if the header is valid. FALSE, otherwise. 

CALLED BY:compBuffer::hasData 

CALLS: none. 

COMMENTS : 


lea Mare ee EER ERR EER REE ERK ARE EERE EA AERA RAE AERA KE KK KK / 


Boolean 
compBuffer::validHeader(compData dataPtr) 
{ 
1f ((dataPtr{[0 
(dataPtr{[l 
ESCULN. TRUE, 
} 
else { 
return FALSE; 
} 


== 'S') && 
CS) ata 


td Ld 
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